Optimising Low-Cost GNSS Positioning for Road Constrained Vehicles Using a Bayesian ﬁlter

Optimising low-cost GNSS positioning for road constrained vehicles using a Bayesian ﬁlter

Andrej Peisker

ORCID 0000-0002-3379-6645

Doctor of Philosophy (PhD) March 2018

Department of Infrastructure Engineering, The University of Melbourne

Submitted for the total fulﬁlment of the PhD Abstract

Co-operative Intelligent Transportation Systems (C-ITS) are a growing industry that have significant implications for the safety and efficiency of on-road vehicle use. Accurate and reliable vehicle positioning is a critical component in C-ITS in order for the component vehicles and infrastructure to interact effectively and information-share for col- lectively enhanced services. Such services include vehicle navigation, collision-avoidance systems, driver assistance and emergency notifications (for example, mobilised if an ambulance needs to pass through the street occupied by the positioned vehicle). A range of existing and emerging technologies have been developed and tailored to improve the accuracy and continuity of positioning for C-ITS, which include Global Navigation Satellite Systems (GNSS), digital road maps, inertial sensor technologies, dedicated short-range communication (DSRC) and other technologies. However many of these technologies are expensive and in many cases prohibitive for large scale roll- out and general public use. Therefore it is important to develop and optimise low-cost vehicle positioning methods to complement more expensive technologies in order to assist with the large scale feasibility of C-ITS, as well as meet individual-scale immediate user needs at viable cost. The subject of this PhD research is optimising low-cost vehicle positioning; that is maximising the accuracy and continuity of position estimates generated from exclusively low-cost data sources. The focus of this research is improving mathematical methods for data fusion (the statistical combination of diverse data inputs for estimation), and a new Bayesian approach is presented for real-time vehicle positioning from the data fusion of low-cost GNSS data and freely available digital road map data. The design of this new method, the Gaussian Cluster Approximate Filter (GCAF), is driven in part by a detailed study of common shortcomings and the causes of failure-modes of existing methods in a range of common urban vehicular situations, in order to be robust to these positioning challenges. Through robust testing it is demonstrated that effective combination of these two low-cost data sources using statistically robust mathematical methods provides substantial improvement to existing approaches, and lessens the need for more expensive sensors in many common vehicular environments. The method is designed to be easily modified to allow fusion with other data sources as future applications may require.

1 Declarations

I declare that: 1. This thesis comprises original work of Andrej Peisker towards the PhD except where indicated and cited in the text

2. Due acknowledgment has been made in the text to all other material used 3. the thesis has fewer words than the maximum word limit, exclusive of tables, maps, bibliographies and appendices

Signed

Andrej Peisker (PhD candidate)

2 Preface

This is to indicate that:

1. This thesis was not carried out in collaboration with others; all original work is the work of Andrej Peisker 2. No work towards the thesis has been submitted for other qualiﬁcations 3. No work towards the thesis was done prior to enrolment in this PhD degree

4. Other than immediate supervisors’ feedback, third party editorial assistance was also provided in preparation of the thesis. Professor Stefan Winter read and provided feedback, and is a professor in the Geomatics ﬁeld at the University of Melbourne and has signiﬁcant experience in the discipline. Dr Adrian Flitney read and provided feedback on grammar, spelling and overall interpretability but not on subject matter content; he has a background in Physics but not Geomatics.

5. No publications or articles have been produced for or from this thesis 6. The PhD research was funded by a Strategic Australian Postgraduate Award (StrAPA), funded in part by the Defence Sciences Institute.

3 Acknowledgments

Sincere thanks go to my family, friends, supervisors and university colleagues who have provided an eternal source of encouragement, inspiration and valuable external perspective during my work on this thesis. This includes my mum, dad, their respective partners and my grandparents for their continued support through this process. Partic- ular thanks go to my supervisors Professor Allison Kealy and Associate Professor Mark Morelande for their wisdom, time, effort and patience shown over a number of years to a student learning the ropes of inter-disciplinary research in a relatively unfamiliar field at a higher level. I also thank Dr Adrian Flitney and Professor Stefan Winter for their time and effort given generously to reading providing feedback on my thesis, and Azmir Hasnur Rabiain for his generous guidance and help at the start of my candidature and throughout.

I would like to acknowledge the Wurundjeri people of the Kulin Nation and their elders past and present, on whose lands the University of Melbourne resides, and on which I was oﬀered, studied for and completed my PhD.

4 Contents

1 Research problem, aims, hypotheses and methodology 10 1.1 Positioning in Intelligent Transportation Systems: Background and Deﬁnitions ...... 10 1.2 Research Aims and Hypotheses ...... 12 1.3 Research Challenges ...... 12 1.3.1 Positioning-Hostile Environments ...... 13 1.3.2 Integrating Road Constraints into Positioning ...... 15 1.3.3 Positioning Performance Requirements of C-ITS ...... 15 1.4 Methodology and Contributions ...... 16 1.4.1 Chapters 2 and 3: Understanding the Accuracy and Continuity Impacts of Positioning Envi- ronments and Road Constraint Integration ...... 16 1.4.2 Developing a New Solution for Road Constrained Vehicle Positioning ...... 17

2 Literature Review 21 2.1 Classifying positioning-hostile environments in urban GNSS-aided positioning ...... 21 2.1.1 Positioning-hostile environments ...... 21 2.2 A review of existing positioning methods and their performance characteristics in urban areas . . . . 25 2.2.1 Some standard methods and statistical models used in positioning ...... 25 2.2.2 Standard methods for integrating road maps into the position estimation process ...... 31 2.3 A quantitative evaluation of the performance of existing positioning solutions ...... 35 2.3.1 Quantitative positioning performance survey across a range of positioning environments . . . 36 2.3.2 Some remarks on the reliability of self-evaluation experiments ...... 39 2.3.3 Conclusions ...... 39

3 Quantifying Low-Cost Positioning Performance Impacts of Vehicular Environments 41 3.1 Introduction ...... 41 3.2 Methodology for measuring impacts of the vehicle’s environment on low-cost positioning performance 41 3.2.1 The Bayesian Root-Mean-Square Error (BRMSE) measure of positioning performance in simulated environments ...... 42 3.2.2 Parameters and design of environmental impact data collection experiments ...... 44 3.2.3 Road Network Representation ...... 44 3.2.4 Models for generating vehicle trajectories and GNSS measurements ...... 46 3.2.5 Estimation Model Co-ordinates and input Parameters ...... 48 3.3 Evaluation of the BRMSE closed-form solution ...... 50 3.4 Numerical evaluation of closed form BRMSE solution to generate positioning error impact dataset . 53 3.5 Discussion of numerical results and comparison with surveyed performance validations ...... 65 3.5.1 Performance impacts of single-factor environments on low-cost positioning ...... 66 3.5.2 Analysis of ﬁndings ...... 67 3.6 Conclusions ...... 68

5 4 The Gaussian Cluster Approximate Filter: A new low-cost solution for statistically robust urban vehicle positioning 69 4.1 GCAF: Algorithm methodology and quantitative models ...... 70 4.1.1 Introduction ...... 70 4.1.2 GCAF: Estimation framework, co-ordinate systems and modeling road constraints ...... 70 4.1.3 State space and initialisation of the ﬁlter ...... 71 4.1.4 Stochastic models for vehicle state dynamics and GNSS measurements ...... 73 4.2 Bringing the elements together: Mathematical solution to the Bayes equation for the GCAF position estimator ...... 80 4.2.1 Gaussian cluster evolution ...... 80 4.2.2 Handling segment boundary crossing during propagation ...... 81 4.2.3 Component sampling regions ...... 83 4.2.4 GNSS measurement integration into positioning ...... 83 4.2.5 Reducing computational complexity of P¯pxi|Ziq for subsequent iterations ...... 86 4.2.6 Modeling and estimating the evolution of vehicle motion state ...... 90 4.2.7 Gaussian cluster merging with multiple model dynamics ...... 90 4.2.8 Gaussian cluster component elimination and re-normalisation ...... 91 4.2.9 Computing position estimates from Pˆpxi|Ziq ...... 92 4.3 Conclusions ...... 94

5 Positioning Performance Evaluation of the Gaussian Cluster Approximate Filter 95 5.1 Introduction ...... 95 5.2 Experimental design and results of the GCAF positioning performance tests ...... 96 5.2.1 Performance evaluation methodology and challenges ...... 96 5.2.2 GCAF Performance test design based on batch Monte-Carlo simulations (Part 1) ...... 96 5.2.3 GCAF Performance test design based on real GNSS-SPP measurement data (Part 2) . . . . . 103 5.2.4 Results summary ...... 122 5.3 Key findings and analysis of the GCAF performance evaluation results ...... 123 5.3.1 Key findings of the performance evaluation tests ...... 123 5.3.2 Analysis of key findings from the GCAF performance evaluation tests ...... 125 5.4 Conclusions ...... 127

6 Conclusions 128 6.1 Conclusions from the research ...... 128 6.1.1 Overview ...... 128 6.1.2 Validation of hypotheses based on key ﬁndings ...... 128 6.1.3 Achievement of research aims ...... 130 6.2 Further Work ...... 131 6.2.1 Multi-sensor integration ...... 131 6.2.2 Accounting for geographical errors in the road map ...... 133

Bibliography 135

Appendices 139

Appendix A Proof of Lemma 1 140 A.1 Nonlinear road constraint integration with GNSS: analytical challenges ...... 140 A.2 Background to this analysis ...... 140

6 A.3 Road segment boundary eﬀects ...... 140 A.4 Loss of Truncated Gaussian Structure following linear propagation ...... 143 A.5 Extension of Lemma 1 to pseudorange measurements and GNSS-deprived periods ...... 145 A.6 Conclusion ...... 145

Appendix B Calculation of GCAF measurement update weights γi,j,k in equation 4.19 146 B.1 Calculating γi,j,k when using pseudoranges ...... 146 B.2 Calculating γi,j,k when using single point positions (SPP) ...... 147

List of Figures

1.1 Process ﬂow diagram for positioning ...... 11 1.2 Positioning environments illustrated ...... 13 1.3 Measured impacts of satellite visibility on positioning ...... 14 1.4 GCAF performance testing zones ...... 19

2.1 Satellite visibility in real urban canyon example ...... 23 2.2 Example of road segment mismatch ...... 24 2.3 Particle ﬁlter posterior distribution for position estimation ...... 29 2.4 Gaussian Cluster posterior probability for position estimation ...... 30 2.5 PSU example method of map matching ...... 33 2.6 PSC example method of representing constrained posterior distribution ...... 35 2.7 Mean positioning error by road constraint integration method and complexity road topology, from aggregated performance data in the literature ...... 37 2.8 Mean positioning error by road constraint integration method and satellite visibility, from aggregated performance data in the literature ...... 38

3.1 Model setup for BRMSE calculations - straight road simulation ...... 45 3.2 Model setup for BRMSE calculations - road with corner simulation ...... 46 3.3 Summary table of chapter 3 results - computed positioning error data based on statistical models . . 55 3.4 BRMSE theoretical average error time series, output of equation 3.2 ...... 55 3.5 BRMSE theoretical average error time series, output of equation 3.2 ...... 56 3.6 BRMSE theoretical average error time series, output of equation 3.2 ...... 56 3.7 BRMSE theoretical average error time series, output of equation 3.2 ...... 57 3.8 Steady state BRMSE values - straight segment vehicle cruise ...... 57 3.9 Steady state BRMSE reduction due to road map data integration - straight segment vehicle cruise . 58 3.10 BRMSE in varying SatVis levels outage scenarios - straight vehicle cruise ...... 58 3.11 Low-cost positioning performance improvement due to road map integration during various levels of SatVis and cruise state ...... 59 3.12 BRMSE values for varying SatVis levels during vehicle cruise state vs right turn scenario ...... 59 3.13 Impacts on low-cost positioning performance due to a vehicle stoppage during low SatVis ...... 60 3.14 Low-cost positioning performance improvement due to road constraint integration (SatVis reduction during stoppage) ...... 60 3.15 Impacts on low-cost positioning performance of a vehicle stoppage during low SatVis ...... 61

7 3.16 Low-cost positioning performance improvement due to integrating road constraints (stoppage during satellite outage) ...... 61 3.17 Impacts on low-cost positioning performance due to linear vehicle acceleration in low SatVis . . . . . 62 3.18 Impacts on low-cost positioning performance due to low SatVis ...... 62 3.19 Low-cost positioning improvement due to road constraint integration (turning corner scenario) . . . 63 3.20 Impacts on low-cost positioning performance impacts due to very low SatVis ...... 63 3.21 Impacts on low-cost positioning performance due to various environments (1) ...... 64 3.22 Impacts on low-cost positioning performance due to various environments (2) ...... 64 3.23 Impacts on low-cost positioning performance due to various environments (3) ...... 65

4.1 Representations of the inner-north Melbourne road network ...... 72 4.2 Visualisation of GCAF weighted component separation at junction crossings ...... 76 4.3 Visualisation of how trajectry constraints influence Gaussian component weights in the GCAF . . . 77 4.4 Visualisation of of Traffic Signal Proximity Zones (TSPZs) ...... 78 ˆ 4.5 Visualisation of the “virtual mean” of the propagated Gaussian component N pxi; µî,j,k, Pi,j,k,Ii´1,j,Si,jq 84 4.6 GCAF posterior predict-and-update example visualisation ...... 86 4.7 Visualisation of the statistical approximation of truncated Gaussian components ...... 89 4.8 The GCAF positioning process cycle ...... 93

5.1 Simulation-based low-cost positioning performance evaluation results (GNSS friendly) ...... 101 5.2 Simulation-based low-cost positioning performance evaluation results (GNSS hostile) ...... 102 5.3 Batch-averaged positioning peak-error of simulation based tests (part 1) ...... 102 5.4 OpenStreetMap (OSM) and Google Earth representations of road map, with testing zones and data superimposed onto OSM ...... 104 5.5 Single-lane road representation used for the GCAF’s along-road position estimation ...... 105 5.6 Haymarket roundabout test zone (Google Earth and OpenStreetMap representations) ...... 107 5.7 Visualised outputs of tested low-cost positioning algorithms computed from same input data (Hay- market, good GNSS) ...... 108 5.8 Visualised outputs of tested low-cost positioning algorithms computed from same input data (Hay- market, temporary GNSS outage) ...... 108 5.9 Time-averaged estimation errors of various low-cost positioning algorithms in the Haymarket real GNSS-SPP data based tests ...... 109 5.10 Particle filter statistical models for stopping distance/time from/at traffic signals ...... 111 5.11 Testing zones and traffic signal locations in the real GNSS-SPP/TSLDI positioning performance impact evaluation experiments (Part 2a Carlton) ...... 112 5.12 Visualised positioning output of PF and GCAF based on real GNSS-SPP/TSLDI data (Grattan/Rathdowne st test) ...... 112 5.13 Positioning error time series results from the Grattan/Rathdowne St real GNSS-SPP/TSLDI positioning performance impact tests ...... 113 5.14 Visualised positioning output of PF and GCAF based on real GNSS-SPP/TSLDI data (Queens- berry/Rathdowne st test) ...... 113 5.15 Positioning error time series results from the Queensberry/Rathdowne St real GNSS-SPP/TSLDI positioning performance impact tests ...... 114 5.16 Time-averaged positioning error results of the real GNSS-SPP/TSLDI positioning performance impact tests ...... 114 5.17 Carlton Gardens test zone (Google Earth and OpenStreetMap representations) ...... 119 5.18 Visualised estimation outputs of tested low-cost positioning algorithms - Carlton Gardens ...... 120 5.19 Positioning error time series results for various algorithms from the real GNSS-pseudorange data based performance evaluation experiments ...... 121

8 5.20 Time-average positioning error results for various algorithms from the real GNSS-pseudorange data based performance evaluation experiments ...... 122 5.21 Results summary table - GCAF positioning performance evaluation tests ...... 123

A.1 How Gaussian components lose Gaussian structure when moving past road shape points and junctions142 A.2 Projection of elliptical GNSS SPP noise distribution onto a road map ...... 143

B.1 Visualising perpendicular road-projections of SPP measurements used to calculate GCAF estimation weights ...... 148

9 Chapter 1

Research problem, aims, hypotheses and methodology

1.1 Positioning in Intelligent Transportation Systems: Background and Deﬁnitions

Co-operative Intelligent Transportation Systems (C-ITS) are a growing industry that have significant implications for the safety and efficiency of on-road vehicle use. A European Union directive defines C-ITS as “systems in which information and communication technologies are applied in the field of road transport, including infrastructure, vehicles and users, and traffic management and mobility management, as well as for interfaces with other modes of transport” [18]. Examples of C-ITS in urban areas include vehicle navigation, driver assistance, collision-avoidance systems and emergency notification for vehicle users [58]. At the core of C-ITS is vehicle positioning, which is the estimation of a vehicle’s position. Because of the location information sharing between a vehicle, other vehicles and infrastructure that defines C-ITS, positioning must be of good quality (accurate and continuously available) to ensure services are effective. Positioning is the estimationx î of the point-location xi of a target object. Estimation encompasses a broad range of methods for inferring the position from data, and a key focus is where incoming data (particularly from sensors) is scarce or noisy. This thesis will examine the problem of low-cost vehicle positioning in urban environments using only GNSS (Global Navigation Satellite System) signals and road map data, and “data fusion” algorithms that combine the data to generate estimates (see figure 1.1). Low cost solutions are widely used in regular personal vehicles as more sophisticated and precise positioning systems (such as carrier-phase signal GNSS) are very costly. It is also common that these systems are exposed to a range of situations (“positioning environments”) which make high estimation accuracy difficult to achieve in practice, such as low satellite visibility (referred to as “SatVis” - the number of satellites in direct line-of-sight from the receiver) and complex road features (see section 1.3.1 for further discussion of “positioning hostile environments”). A common estimation method for such low-precision data situations is statistical Bayesian estimation using data fusion, which involves using probability distributions over a state space (e.g. position/velocity). Two key positioning performance attributes that will be defined and referred to are accuracy and continuity. These may be measured by a range of metrics, though accuracy will be measured via positioning error i “ T |xi ´ xî| ” pxi ´ xîq pxi ´ xîq, and the continuity by reference to positioning outages by frequency/duration (in Hz or seconds) [42, 56, 58, 19]. A ”positioning outage” is here defined as an incidence of positioning error exceeding aa (typically known) level such that the service provided by the positioning can no longer be effectively provided during the error. While error cannot be normally measured in real time since the ground truth position is unknown, it may be estimated by the uncertainty which is related to the variance in the estimated position, when probabilistic estimation is used. Note that the term ”outage”, when used on its own in this thesis, will refer to a

10 positioning outage, whereas a satellite visibility of zero is referred to as a ”GNSS outage” and satellite visibility between 1 and 4 is referred to as a ”partial GNSS outage”. Outages can lead to reduced performance, or temporary failure, of the C-ITS application if the frequency and/or duration are significant enough [5, 42]. Failure of an C-ITS application can have significant life or cost-critical consequences since many applications are for road safety on which users depend. The accuracy and continuity of leading positioning solutions are currently not good enough to provide service for the desired range of C-ITS applications [42, 58, 23]. With over 60 million new cars on the road each year, and 0.7% and 2% of GDP spent on traffic congestion and vehicle collisions respectively, there is great incentive to increase the efficiency, safety and effective communication between vehicles in private transport by improving positioning accuracy and continuity [58, 47]

Figure 1.1: The relationship between data fusion, positioning and C-ITS. Modern positioning solutions for vehicle users integrate (“fuse”) a range of data sources.

The deﬁnition of urban areas in this thesis include any built-up area with an existing road network. These are particularly important in low-cost GNSS-aided positioning because buildings, particularly taller concrete or

11 glass structures create blockages to GNSS signals, depriving GNSS-aided positioning methods of quality data which reduces accuracy [42]. Providing a low-cost estimation method robust to this “urban canyon” effect is one of the key research problems in positioning, and is central to this thesis. Key applications of vehicle positioning that would benefit from a low-cost robust positioning solution include personal navigation for private vehicle use, minimisation of emergency response delays, improved courier delivery efficiency and driverless vehicle technologies [58, 5]. It is therefore important to understand the causes and manifestations of vehicle positioning error. Understanding positioning error behaviour and its driving factors in urban environments can lead to targeted positioning solutions that reduce error where it has the greatest impeding effect. It is also important to develop a clear understanding of the assumptions and models used in data fusion in order to address inefficiencies in existing solutions, and understand the state of present research and build on it. This includes models for GNSS noise, vehicle dynamics and models for integrating digital road maps into positioning, which leads to the central aims and hypotheses.

1.2 Research Aims and Hypotheses

The aims of this thesis are 1. To generate a more comprehensive understanding of existing positioning methods and more robustly quantify the causes and characteristics of error in GNSS-aided urban vehicle positioning 2. To evaluate the usefulness of road constraints based on maps as a key data input for low-cost vehicle positioning, and how their usefulness is impacted by the method of road constraint integration into positioning 3. To apply understanding of performance impacts of road constraints on positioning, and the causes of positioning error, to create a performance-improved (compared to existing methods) and well-tested positioning solution with a statistically optimised method for integrating road maps as a key data source for improving positioning. 4. To develop a data fusion framework that extends the scope of data sources which can be integrated into positioning (such as turn restrictions and traﬃc signal locations) In the context of data fusion driven estimation, positioning solutions will be within the scope of data fusion algorithms which process information inputs zi at time ti and generates position estimatesx ˆi as output in real-time. Data fusion is the statistical processing of information used to compute estimates using probabilities. The research hypotheses are

1. That vehicular transport in urban areas can be decomposed into a set of discrete positioning environments which have distinct and quantifiable characteristics that influence estimation accuracy and continuity 2. That road constraints are a critical information source in positioning and can significantly reduce errors in a range of environments (particularly those which are hostile to positioning), and that the method of their integration is highly influential on the level of performance improvement. 3. That a broader and more detailed quantitative understanding of positioning environments and their performance impacts can effectively direct the focus of positioning research to develop better performing low-cost vehicle positioning solutions 4. That integrating a broader range of data sources into positioning will improve performance metrics such as accuracy and continuity

1.3 Research Challenges

Two central research challenges exist for GNSS-aided vehicle positioning in urban areas.

12 1.3.1 Positioning-Hostile Environments This thesis will define a model that refers to vehicles as being in a particular positioning environment at any given time. The positioning environment at time ti is defined as the set of all parameters which influence positioning accuracy and continuity in practice at time ti. This may include the SatVis level, the motion state of the vehicle and traffic conditions, or the amount of reflective surfaces in the surroundings which may degrade GNSS signal integrity (though this list is not exhaustive) (see figure 1.2) [42, 25]. These environments may range from being net-favourable to net-detrimental for achieving good accuracy and continuity, and are continuously changing as the vehicle moves through the road network. Net-detrimental environments are referred to as positioning hostile environments (PHEs), and are a key research problem in this thesis. The positioning performance impacts of PHEs must be overcome in order to achieve accuracy and continuity. One challenge presented by PHEs such as low SatVis or reflected signals (“multipath”) is positional ambiguity. Ambiguity is the level of uncertainty in positioning, resulting from a range of possible candidate position estimates being equally or near-equally viable based on the input data and resulting probability distribution. [42, 26, 57].

Figure 1.2: A example vehicle trajectory highlighting a range of positioning environments typically encountered in urban areas. Data from Google Maps, Melbourne’s inner north.

Urban canyons are an important and common PHE, including built up areas where the average GNSS SatVis is reduced due to man-made or other physical signal obstructions. In many cities, high-rise business districts reducing GNSS to low levels is common. For example, in the Hong Kong district of Wan Chai, the SatVis necessary for stable positioning accuracy (3 or more) for navigation is found only in 20% of areas, and SatVisď1 in 60% of on-road areas [14, 8]. Wu et al (2005) found that over 80% of the positioning outages that led to navigation failure in Hong Kong were caused by SatVis reduction in urban canyons [14, 8]. Several studies have shown that SatVisď2 conditions often lead to error divergence in standalone GNSS positioning or when fused with road maps, therefore disrupting C-ITS applications (see ﬁgure 1.3 for a visual example of error divergence) [7, 13, 42]. A signiﬁcant focus

13 is therefore on positioning in low-SatVis circumstances. A broad discussion of positioning sensors and their performance characteristics in a range of PHEs (not just urban canyons) is given in [5]. Some discussion of topological conditions and their impact on positioning is given in [42, 50], and some discussion on the impacts of erratic vehicle motion (such as that caused in heavy traﬃc) is given in [26, 9].

Figure 1.3: Vehicle positions computed using an Extended Kalman Filter with pseudorange data as inputs collected using a Leica GS10 GNSS receiver. Data collected from a field-data collection journey in inner Melbourne. The top-left image filters all pseudoranges collected by the receiver. The other images show position estimates based on a reduced input dataset where pseudoranges were artificially removed from the original dataset so only the indicated number remained as inputs. This was done to simulate and visualise the effect of urban canyons (which would typically see similar low numbers of satellites visible over prolonged periods)

PHEs present a signiﬁcant research challenge because in addition to degrading positioning performance, their performance impacts are not robustly understood or modeled. In chapters 2 and 3 the quantitative positioning performance impacts of a range of PHEs and their characteristic inﬂuences on positioning error are discussed.

14 1.3.2 Integrating Road Constraints into Positioning Secondly, integrating road constraints into positioning in an optimal way is a complex problem with no clearly optimal solution in the literature. Road constraint integration is any method of incorporating the road network map and auxiliary data as part of the input data used in data fusion for positioning, and can be particularly helpful in vehicle positioning in urban areas, where the vehicle is almost always road-bound [42, 56]. As discussed in chapter 2, many solutions exist for integrating road constraints into positioning, where the diversity largely results from the competing priorities of mathematical accuracy and computation speed. In chapter 4, it is demonstrated that a mathematically exact solution to the problem of GNSS/road constraint integration cannot meet real-time demands and must therefore be at least partly approximated. This creates a research challenge because there is currently no clear consensus on the best method of optimising this trade-oﬀ. Particular attention must be given because road constraint integration is known to be detrimental to positioning if the mathematical accuracy of the GNSS/road map integration method is too simplistic. This is due to bias errors, occurring when ad-hoc mathematical procedures cause systematic skewing of the estimated position output [42, 56]. Part of the challenge is therefore to broadly survey the existing C-ITS road constraint integration literature and robustly quantify the impacts of the method on accuracy and continuity performance. Constraint integration needs to then be optimised based on the ﬁndings of this survey, or a new approach formulated to create an improved positioning solution which improves accuracy and continuity to meet basic C-ITS needs. The challenge includes determining a method for the solution to be validated in a robust and bias-free manner, since this has not been adequately achieved in the literature [42, 56]. In chapters 2, 3 and 5 the quantitative performance impacts of a range of road-constraint integration methods are examined, and compared with those of a new method formulated in chapter 4.

1.3.3 Positioning Performance Requirements of C-ITS The positioning accuracy/continuity requirements are diﬀerent for each class of C-ITS. A list of the basic quantitative requirements for positioning for C-ITS is provided below to contextualise the scope of the research problem. A seminal example of C-ITS is GNSS-aided navigation which can allow for brief periods of 10-20m errors, though optimally functions at 5-10m errors, while collision avoidance systems require high-frequency sub-metre accuracy [58]. No application can function in the presence of error instability, or growth, and generally no C-ITS can function optimally with average ą10m errors [42]. Even these looser targets have not been met by leading methods, as error growth and magnitude events are still observed in PHEs. Containing error growth in PHEs, particularly in low-SatVis environments, is an achievable and necessary minimum objective for C-ITS. Another key requirement is that positioning output is computed in near-real time, that is position estimation output is generated at a frequency equal to the reciprocal of a prescribed sampling period ∆t “ ti ´ ti´1 and with delay less than one sampling period. The sampling period varies between applications, is typically 1s in the case of navigation, and ă0.1s for collision avoidance and other rapid-response applications [58]. A research challenge is therefore to balance the competing requirements of real-time output speed and estimation accuracy which demands greater computation and reduces speed.

Existing positioning solutions do not achieve the performance requirements required for many C-ITS applications. The unreliability of performance evaluation data based on primary source validations makes it diﬃcult to gauge the positioning accuracy/continuity levels of leading solutions [42, 56], particularly in PHEs where experiments are less commonly undertaken for simplicity. For example, the positioning accuracy/continuity service requirements for GNSS-aided navigation are met in positioning friendly circumstances (high SatVis, steady motion), but satisfactory error levels have not been demonstrably achieved in a broad range of PHEs [42, 56, 5]. A focus on positioning error containment methods in urban canyons over the last decade has seen positioning continuity improvements, but not to satisfactory levels in SatVisă2 environments (see chapter 2) [7, 17, 13, 23]. While urban canyon eﬀects have been studied to some extent, the combination of urban canyons with other factors (such as erratic motion or topologically complex areas) has not been widely studied and no conclusive data on these aggregate

15 eﬀects exists [42, 56]. As referred to in the aims, one objective of the work in chapter 4 is to optimise the low-cost data fusion method of GNSS data and road constraints mathematically, and reduce errors in a range of vehicular environments to restore feasibility to some services in a range of PHEs (e.g. urban canyons). Clearly limitations exist on such a low-cost solution which cannot provide viable solutions for high-demand services such as collision avoidance with only GNSS and road constraints. However, the fusion framework provided is intended to be extensible to other data fusion contexts that may include higher cost sensor architectures and thus resource future solutions to other C-ITS positioning problems.

1.4 Methodology and Contributions

Chapters 2, 3, 4 and 5 will address the research aims outlined in section 1.2. Chapters 3, 4 and 5 will test the research hypotheses. Chapters 2 and 3 address the aim of developing a detailed quantitative understanding of the impacts of vehicular environment on positioning performance and how road constraints influence these impacts (aims 1 and 2, hypotheses 1, 2 and 3). In parallel, these chapters will also closely examine the effect of the data fusion method, in particular the method of road constraint integration, on positioning performance. Chapters 4 and 5 address the aim of developing and testing a new positioning solution optimised for positioning- hostile urban environments (addressing aim 3, hypotheses 2, 3 and 4). A new theoretical framework for efficiently integrating road constraints with sparse GNSS is introduced in chapter 4, which includes original methods of integrating traffic signal data and one-way/turn restrictions (addressing aim 4, hypotheses 3 and 4) and validated with real and synthetic data in chapter 5 (addressing aim 3, hypotheses 3 and 4). The key novelty of the method is the precision with which the complex positional probabilities can evolve over time while still being computationally feasible due to minimal analytical transformations. It is argued that existing methods achieve lower levels of precision in representing positional probabilities often through significant simplifications for convenience, to the point of introducing biases into positioning. In chapter 6 the conclusions of the work and future directions for this research are presented. Some more details on the individual chapter contributions are provided below.

1.4.1 Chapters 2 and 3: Understanding the Accuracy and Continuity Impacts of Positioning Environments and Road Constraint Integration In chapter 2 a literature review is presented including a survey of existing positioning methods. This survey provides a quantitative comparison of the performance of over 50 GNSS-aided positioning algorithms using the primary source validation data. The literature survey has been designed to cover as broad a range of positioning environments as possible, as well as a large sample of algorithms which integrate road constraints and those which do not. The summary data presented in chapter 2 therefore contains real-data verification of the positioning performance impacts of environments and the use of constraints, which has not been achieved by previous surveys. The limitations of aggregating data from independent and inconsistent primary sources is explained in chapter 2. The limitations include issues of test parameter selection bias, consistency of data fusion methods tested, and statistical significance of results. To address these limitations, a set of theoretical model-based performance calculations is designed and numeri- cally evaluated in chapter 3. Using physically verified stochastic models and assuming a linear-quadratic Bayesian estimation method, the calculations compute theoretical average positioning errors over a broader range of positioning environments than exists in the primary source positioning performance evaluations. Linear-quadratic estimation is a widely used technique in positioning, and includes the Kalman Filter, which is tested in the calculations [27]. A new error metric, the Bayesian Root-Mean Square Error (BRMSE), is formulated to allow an exact closed form solution to the average error metrics in all environments. Competing metrics such as the Bayes Mean

16 Error, or Cramer Rao Lower Bound, cannot be evaluated exactly over the desired test scope and thus jeopardise statistical significance or practical time-feasibility [52]. The calculations also compare methods which integrate road constraints and those which do not, using a road- constrained Kalman Filter variety. A broad and statistically significant dataset on error-driving factor impacts is generated by design because the theoretical averaging eliminates random noise effects from the time series outputs. This sort of theoretical analysis of the quantitative environmental/road constraint impacts on positioning performance has not previously been done. The theoretical approach removes the significant practical barriers such as the time, cost and experimental design constraints of collecting real data for the same purpose. To summarise, the key contributions from these chapters include:

• Chapter 2: Quantitative survey summary data evaluating the positioning accuracy/continuity impacts of PHEs and road constraint integration from primary source validations • Chapter 2: A summary of existing positioning methods including a theoretical discussion of the protocols for road constraint integration • Chapter 3: A new theoretical metric for evaluating the positioning error of linear-quadratic estimation methods with linear-Gaussian models (the BRMSE), and its general solution • Chapter 3: Original positioning performance data based on theoretical models, quantifying the positioning accuracy/continuity impacts of a broader and more representative range of application-typical PHEs as well as road constraints

1.4.2 Developing a New Solution for Road Constrained Vehicle Positioning Based on an in-depth survey of existing data fusion methods for low-cost positioning, the second and third research aims are addressed by formulating a new algorithm for road-constrained GNSS-aided vehicle positioning, the Gaussian Cluster Approximate Filter (GCAF), in chapter 4. This algorithm is robustly tested with real data and simulations in chapter 5. The design of the GCAF (chapter 4) is centred on achieving optimal accuracy/continuity performance in positioning-hostile environments by integrating GNSS pseudoranges and road constraints (data fusion). The formulation makes use of the findings of chapters 2 and 3 regarding the positioning performance impacts of environments and road constraints by focusing on the constraint integration methods which were proven to perform better in PHEs, especially low SatVis conditions. Chapter 4 contains a mathematical proof that the statistically exact integration of road constraints with linear-Gaussian modeled measurements is not possible in real-time (due to exponential computational complexity growth), assuming a Gaussian-sum positional prior with non-zero covariance components. This implies that statistically optimal road constraint integration with GNSS requires an approximate solution to the Bayes equation (equation 2.1), and justifies the approximate approach of the GCAF. The key new design component of the GCAF is the precision-preserving recursive-Bayesian approximation method for integrating non-linear constraints (including map, traffic signal and turn constraints) with Gaussian-error pseudorange measurements/dynamics in real-time. The recursive Bayesian approach uses a time-evolving probability distribution (the posterior Ppxi|Ziq at time ti) which represents positional uncertainty over the vehicle state xi to guide estimation. The vehicle state is a vector of time-evolving physically relevant parameters estimated in parallel at each time (including the position). Ppxi|Ziq is computed using Bayes’ equation:

17 • Λpzi|xiq is a probability density function representing the likelihood of observing data zi given the true state xi

• Φpxi`1|xiq is a probability density function representing the likelihood of the state evolution process xi´1 Ñ xi occurring between times ti´1 and ti

The efficient approximation protocol of the GCAF (discussed in detail in chapter 4) aims to improve on existing approximate solutions to non-linear constraint integration for positioning which are identified in the literature as risk-prone to bias errors, especially in PHEs, due to imprecise approximation protocols [42, 56, 23, 13], see chapter 2. The theoretical approach to positioning performance optimisation via estimation bias-minimisation is arguably more cost effective than hardware-based solutions for developing a solution over a broad range of PHEs. Hardware- based solutions include the integration of multi-sensor platforms, as well as sensor performance optimisation. Multi- sensor integration is the combination and joint data processing of a range of sensors, which is often used in positioning to effectively reduce errors in PHEs [58, 1, 3]. However, this approach is more cost intensive to develop, particularly if the solution is to provide benefit in all PHEs [58]. For example, integration of inertial sensors (INS) provides relief in low GNSS conditions, but may not identify a road segment mismatch in a topologically complex area without an estimation bias-minimising road constraint integration scheme [3]. Sensor performance optimisation includes the enhancement of sensor accuracy through the reduction of biases and noise errors in measurement readings, through better calibration or improved hardware. This also imposes cost and time constraints and may not present an effective solution over a broad range of PHEs, though may provide relief in some specific PHEs. For example, improved GNSS accuracy via carrier-phase processing improves accuracy when GNSS signals are available, but is ineffective in SatVis=0, and provides little information in SatVis=1 [25]. In chapter 5 the GCAF is tested robustly in a range of performance evaluation experiments using both real and synthetically generated data. The experiments are designed to produce a statistically significant validation dataset of GCAF positioning accuracy/continuity over a broad range of positioning environments including common PHEs. The performance metric used was position error, computed from the estimated GCAF position and ground truth from available road map/GNSS data inputs. Experimental design using synthetically generated data overcome validation shortfalls identified in chapter 2 by reducing selection bias (randomised vehicle trajectories, environmental parameters and simulated GNSS data) and increasing statistical significance (large sample sizes of randomised trajectories). Randomised trajectories were generated in the inner-northern Melbourne suburbs based on the OpenStreetMap (OSM) road network database, using randomised starting points and trajectory selection for each trial.

18 Figure 1.4: Visualisation of the testing zones (circled) used in the real-data positioning performance tests of the GCAF described in Chapter 5. The overlayed blue points show the route taken by the vehicle over the entire journey and are position estimates generated by an Extended Kalman Filter.

Real-data based performance validation experiments complemented the synthetic validation, by testing the physical models against the reality in application and real PHEs. GNSS signal data was collected during a vehicle excursion in the inner-northern suburbs of Melbourne, designed to include a range of positioning environments including urban canyons, topologically complex areas and high-traffic/motion unsteady areas. The GCAF generated position estimate output based on the GNSS data collected and its accuracy/continuity tested relative to the ground truth positions. Ground-truth positions were computed by real-time-kinematic (RTK) differential GNSS (DGNSS), and have cm-level precision in certain areas. The GCAF was performance tested using subsets of the collected GNSS data from two particular regions, the Haymarket roundabout in North Melbourne and the vicinity of Carlton Gardens (see figure 1.4). These were selected because 1) the RTK was high enough quality in those regions to allow cm-level ground-truth precision, and 2) a range of PHEs were present in a relatively small geographical area. Both raw GNSS pseuodranges and SPP based on the raw pseudoranges were used as inputs to the GCAF. To summarise, the key contributions of these chapters include:

• The formulation of a new positioning solution (GCAF) based on a new approach to road constraint integration with GNSS signals for bias-minimising estimation • A mathematical proof of the real-time impossibility of statistically exact integration of road constraints with linear-Gaussian modeled measurements, assuming a Gaussian-sum positional prior with non-zero covariance components • A theoretical framework for the statistically optimal non-linear integration of road constraints with sensors

19 using linear-Gaussian measurement models • A new statistical model for integrating a broader range of auxiliary road constraint data, including traﬃc signal locations and turn restrictions • A statistically signiﬁcant performance validation dataset of the GCAF based on a variety of experiments including real/synthetic measurement or trajectory data encompassing a broad range of positioning environments and comparisons with various existing methods.

20 Chapter 2

Literature Review

This chapter contains a review of the GNSS-aided positioning literature specific to vehicles in urban road-constrained environments. The objective of the chapter is to provide background theory and context for the theoretical contributions in chapters 3, 4 and 5. The review is divided into two parts. Firstly, a brief classification of the main causes of positioning error for low-cost urban vehicles identified in the literature survey is presented in section 2.1. The second (main) part is a broad quantitative performance survey of existing positioning algorithm solutions based on their self-evaluation data as well as review papers. This performance survey analyses in detail the performance impacts of road constraint integration and vehicular environments on positioning error, and identifies key shortcomings and deficiencies in existing solutions which the proposed new solution in chapter 4 intends to address.

2.1 Classifying positioning-hostile environments in urban GNSS-aided positioning

While existing low-cost positioning solutions provide continuous service much of the time in benign situations (e.g. high SatVis), significant errors (ą10m) are common in more hostile environments. For example in highly built-up urban environments, it is uncommon for low-cost solutions to provide continuous service, encountering frequent divergences in error due to (largely) SatVis reduction [13, 57, 14]. The existing body of solutions does not optimise fusion of GNSS and road constraints to maximise accuracy/continuity in hostile environments, particularly in intermediate SatVis=2 or 3 scenarios where performance can vary greatly between methods. In these scenarios, more robust data fusion of road maps can provide significantly greater accuracy, continuity and reduced incidence of positioning outages/error growth over time [3, 19, 58, 42, 56] (see section 2.3.1). Below is a broad classification of positioning-hostile environments according to the main error-driving parameters that have been identified followed by a brief discussion of why existing low-cost solutions do not achieve optimal performance in the presence of these parameters. The purpose of this discussion is 1) to introduce a set of core concepts which will be frequently referred to in the main review of existing positioning solutions in section 2.3, and 2) to develop a broader theoretical understanding of the main urban positioning error drivers which will help to justify the design of the new data fusion solution.

2.1.1 Positioning-hostile environments Urban areas often present hostile environments for positioning with multiple error-driving factors overlapping at any given time, presenting a signiﬁcant challenge for the positioning solution. It is important to understand the characteristics of these factors in isolation in order to identify the common causes of deﬁciency in existing solutions, prior to considering the impact of the factors together.

21 Urban canyons Urban canyons are the predominant challenge for GNSS-aided positioning services, particularly in highly built-up inner-city areas. GNSS-aided positioning (including stand-alone GNSS) generally achieves stable service-threshold accuracy/continuity when SatVisě4, which permits a viable least-squares solution for the vehicle position [25]. Lower SatVis results in positional ambiguity as a number of candidate positions are viable without adequate signals or other auxiliary information to “pin-down” (constrain) a solution in a mathematical sense. Mis-identification of vehicle motion states can also easily occur in these situations, for example an incorrect trajectory direction, or a turn mis-identified as an acceleration resulting in additional error drivers [14, 57, 8, 13, 42, 4]. Urban canyons are a critical barrier because no existing positioning solution integrating only GNSS and road maps in highly dense urban canyons (SatVis=0 or 1 which can occur frequently in built-up metropolises such as Hong Kong, see figure 2.1) is able to achieve an error-stable position with ă20m errors that do not continue to grow with time [42, 12, 13, 56], therefore precluding most C-ITS services from operating. However, less densely built up areas where SatVis=2 and 3, performance varies much more greatly between solutions depending on other factors such as the road geometry/topology, vehicle motion state and the method of data fusion (see section 2.3.1 for discussion of multiple-factor environments). Performance in SatVis=2 and 3 may range from diverging error (ą20m and growing) to periods of stable and low error (ă10m), see section 2.3.1. This can be observed in the empirical positioning datasets visualised in figure 1.3 from which estimation accuracy/continuity may be compared at different SatVis levels. Line-of-sight obstruction to GNSS satellites is not the only error driver of urban canyons. Reflective surfaces which are frequently present in urban areas (e.g. exteriors of buildings, road surfaces) can also reflect signals resulting in a non-direct path from the source to receiver (“multi-path”). This can lead to an erroneous estimated position since it is difficult to identify if a signal has been multipath-affected, and thus it may be assumed to be unaffected leading to calculation biases [25].

Topologically complex road-network features When road maps are integrated as a state constraint into positioning, an increase in positioning error is also frequently seen in the vicinity of topologically complex road-network features (TCRNFs). A TCRNF is defined as any region with more complex connectivity features, or unusual structure. This can include intersections, overpasses, multi-lane roads and regions where the road grid is dense such as inner-urban areas (particularly where intersecting laneways thread through vehicle-bearing roads). These features can create ambiguity as a range of candidate trajectories are possible, increasing the probability of a mis-estimated position, compared to, e.g. a straight isolated road in a country area [50, 57]. This is also identified in the literature survey of Yu et al, who also observed that “although most of the GPS/DR based map-matching algorithms work well in open area conditions with sparse road networks, none of them are specifically designed to face the challenges of positioning in urban area with complicated road networks and frequent GPS signal blockages” [57]. Typically, positioning error increases temporarily as a “spike” while the vehicle traverses the TCRNF [57, 42, 43]. The mismatch is typically identified shortly following departure from the feature resulting in only brief service outage (u10m errors, assuming SatVis is not low), though dense road networks may cause prolonged mismatch error as no relief is provided from the trajectory ambiguity (see figure 2.2) [14, 13]. SatVisď2 situations tend to significantly increase and prolong the error peak when traversing a TCRNF, since the poorer GNSS information has less capacity to correctly and unambiguously identify the mismatch [13, 14]. Errors in this case may plateau at 10s of metres, potentially over minutes [14, 57]. Studies investigating the positioning impacts of topological network complexity can be found in [14, 8, 50, 57].

22 Figure 2.1: Example of the impact of urban canyons on satellite visibility in heavily built up Hong Kong, as measured by the frequencies of SatVis levels occurring in each of its districts (as percentages of the districts). Image sourced from Chao et al 2001 [8]. SatVis is the number of satellites in direct line-of-sight of the vehicle receiver.

23 Figure 2.2: Example of “segment mis-match” in low-cost positioning with road constraints. The matched trajectory identiﬁes the wrong segments due to the density of the road network combined with low satellite visibility reducing accuracy. Image sourced from [14]

24 Non-uniform vehicle motion The review of studies have shown that positioning error tends to increase during periods of non-uniform or unpredictable vehicle motion [42, 57, 56, 30, 9]. This includes turns, stoppages, quick accelerations, as well as unsteady stop-start motion such as in dense traffic. Errors will typically peak over a short period during the unsteady motion, with the peak magnitude correlating largely with the magnitude of the acceleration experienced by the vehicle (either linear or centripetal) [26, 6, 9]. This is due to the positional ambiguity in the resulting GNSS signals, as the change in motion is typically not anticipated in most positioning algorithms and may be erroneously interpreted as statistical noise, favouring the inference of steady motion. Vehicle turns are typically short (several second) manoeuvres with relatively low accelerations and tend to cause a small bump in moderate-good SatVis that typically will not cause service outage, similar for the other manoeuvres listed above [9]. However, when compounded by low SatVis, error peaks associated with non-uniform motion can be greatly amplified and cause service outage, since the positional ambiguities caused by the unexpected GNSS signal changes during manoeuvres are not corrected with quality measurements [57, 14]. This may cause an error divergence where, for example, a turn is not identified and a heading error results until corrected when SatVis increases [14]. According to Chen et al’s 2005 survey, 22% of mismatches in map-matching algorithms resulted from complex or unpredictable vehicle motion which was inadequately anticipated or modeled by the algorithm’s dynamical model [14]. Other studies examining unsteady motion as a positioning-hostile factor include [26, 53, 42, 56, 30, 34, 14]. s Kirubajaran and Bar Shalom (2000) introduced a quantitative criterion Λ “ σa T {σw which indicates a high likelihood of error due to manoeuvring motion Λ ą 0.5 if a rudimentary motion model was used, where σa is the RMS acceleration magnitude over the manoeuvre, T is the sampling period and σw the modeled GNSS measurement noise intensity [30].

2.2 A review of existing positioning methods and their performance characteristics in urban areas

In this section the key performance-evaluation findings from the literature survey of existing positioning methods are summarised. The broader classes of standard methods are introduced in this section, and then compare their performance quantitatively in section 2.3.1. The classes of positioning algorithm tend to be divided along lines of 1) the statistical models and approximations used to estimate the positional uncertainty and 2) the method used to integrate road maps into the positioning. As clearly identified in the literature survey, positioning error can also be largely attributed to the design itself of the positioning algorithm, in addition to the external factors discussed in section 2.1. Design elements that can contribute to error include inaccurate models for the GNSS data or vehicle motion, imprecise approximations used in determining positional uncertainties, and importantly, a statistically unsound or biasing method of integrating road maps into the estimation of position. It is concluded that no existing method adequately addresses these three modes of design failure, and use the quantitative findings to guide the design of the new method in chapter 4.

2.2.1 Some standard methods and statistical models used in positioning The broadest class of standard methods used widely in data fusion for positioning are ﬁlters [25, 12, 23]. Filters are a Bayesian statistical method of reducing noise in successive GNSS signals and, depending on the method, potentially combining them with models for vehicle motion and with road map data to produce a statistical uncertainty distribution (denoted Ppxi|Ziq) over the vehicle state xi. This process is referred to as data fusion. Ppxi|Ziq is computed using Bayes theorem :

ş 25 where • X is the set of all possible vehicle states (which may include constraints)

• Λpzi|xiq is a probability density function representing the likelihood of observing data zi given the true state xi

• Φpxi`1|xiq is a probability density function representing the likelihood of the vehicle’s state evolving from xi´1 Ñ xi between times ti´1 and ti

The vehicle state may include just the position xi but may also include auxiliary variables that are estimated in parallel, which may serve to improve the quality of estimates [25, 27]. These may include the speed/acceleration of the vehicle, its altitude and also the bias of the vehicle’s GNSS receiver clock. Filtering therefore uses statistical techniques to reduce noise in the GNSS signals at time ti via the smoothing effect of corrections from past position estimates pxi´1, xi´2, ...q and concurrent auxiliary-variable estimates. Mod- ern positioning methods which do not employ filtering at some point in the positioning process are rare, and the best performing methods are generally all filters [42, 12, 58].

Kalman Filter The Kalman Filter is one of the oldest and most cited examples of a positioning filter method [27, 42, 12, 25]. It estimates positions based on a model for the positional evolution and measurements (e.g. GNSS), estimating positions from a computed Gaussian probability distribution [25]. Since its introduction in 1960, it has had been the basis of a broad range of positioning algorithms which have generally sought to extend the theoretical framework of the KF to develop solutions to specific positioning problems [42, 12]. Its popularity derives largely from the simplicity of its statistical models which are linear, therefore ensuring rapid computation speed which ensures the high-frequency real-time positioning output requirement for many C- ITS applications is met. In low-cost positioning, the input models for the KF include the linear dynamical model for the vehicle (equation 2.3), and the model for GNSS measurements (equation 2.2) [25]. These models are used to determine the solution of the filter equation 2.1 and therefore the position point-estimatex î, where equation 2.3 determines the distribution Φ and equation 2.2 determines the distribution Λ. The multiplier matrices Hi and Fi do not depend on the quantities zi or xi, and the additive errors have known probability distribution. The error terms in a KF are always Gaussian so that the models are linear-Gaussian for efficiency of computation. The point estimatex î is the mean of the posterior distribution because this minimises the expected squared estimation error, thus the KF is referred to as a quadratic linear estimator. This estimate may always be expressed in exact form when models are linear-Gaussian, requiring no simplification or approximation to be efficient [25]:

zi “ Hi xi ` wz,i (2.2) , where

• Hi is the measurement matrix

• wz,i is the zero-mean Gaussian “measurement noise” term at time ti

xi`1 “ Fi xi ` wx,i (2.3) where

• Fi is the propagator matrix

• wx,i is the zero-mean Gaussian “process noise” term at time ti The KF is often used in combination with road maps to improve positioning performance, and a variety of methods for this exists with no obvious best solution (see section 2.2.2 for discussion).

26 Extended Kalman Filter Linear-Gaussian models for vehicle dynamics or GNSS measurements may not always be deemed optimal. This may occur, for example, when the true models are not close to linear, e.g. the model equations 2.3 and 2.2 with ﬁxed Fi and Hi would be a poor approximation for the true models Φ and Λ in equation 2.1. An example is when measurements zi are in the form of pseudo-ranges (distance estimates from satellites), which are described by equation 2.4 and cannot be expressed in a linear form of the vector pxi, yi, zi, ∆tiq [25, 44]. This often presents a problem because solving equation 2.1 with non-linear models is often impossible in real-time due to the compuational burden [44, 25]. The Extended Kalman-Filter (EKF) presents a popular solution to this problem.

2 2 2 ρi,j “ pxi ´ Xi,jq ` pyi ´ Yi,jq ` pzi ` Zi,jq ` c ∆ti ` i (2.4) b L pzi ´ z0,iq “ Hi pxi ´ xi q ` wz,i (2.5) where

• zi “ pρi,1, ..., ρi,SatV isi q is the set of observed pseudorange measurements at time ti • z ρ0 , ..., ρ0 is the set of predicted pseudorange measurements at time t 0,i “ p i,1 i,SatV isi q i

0 L • ρi,j “ |Xi,j ´ fpxi q| • fpxq maps a point x in the state space to its position in Earth-Centred-Earth-Fixed (ECEF) co-ordinates. The ECEF system is an 3-D orthogonal system of spatial co-ordinates with the origin at the Earth’s centre, Z the polar axis, and the X axis intersecting the Earth’s surface at the prime meridian.

• Xi,j is the position of satellite j at time ti in ECEF co-ordinates

L • xi “ Fi xi´1, where xi´1 is the state estimate at time ti´1

• wz,i is the zero-mean Gaussian “measurement noise” term at time ti

The EKF uses model linearisation to make the non-linear input models have the same form as equations 2.3 and 2.2, however the matrices Fi and Hi are not fixed but vary with time, which tends to be a better approximation of the true models than fixed matrices [44, 12]. Converting the non-linear models to this form allows computation to be as efficient as for a KF, though the solution to equation 2.1 is then approximate with respect to the true models. The process of linearisation involves taking the first-order Taylor expansion of the non-linear models around a L non-fixed linearisation point xi , see equation 4.8, and approximating the true models with this expansion, where the input matrices are given by equations 2.6 and 2.7.

B Fi “ φpxiq (2.6) Bxi x xL ˇ i“ i ˇ ˇ is the linearisation of φ if the the non-linear state evolutionˇ model is given by xi`1 “ φpxiq ` i B Hi “ λpxiq (2.7) Bxi x xL ˇ i“ i ˇ ˇ is the linearisation of λ if the non-linear measurement modelˇ is given by zi “ λpxiq ` i.

While achieving computationally eﬃciency, the EKF has been shown to understate estimation uncertainty, leading to bias errors in positioning [44, 15, 25]. Like the KF it has many methodological variants such as the GCM and IMM ﬁlters, as further discussed in sections below.

27 Particle filters The particle filter (PF) was introduced in 1993 by Gordon, Salmond and Smith (1993) [21] and has become a very popular method particularly when models for the vehicle dynamics/GNSS are nonlinear. The PF approximates the non-linear exact posterior solution to the filter equation 2.1, i.e. Pipxi|Ziq using a time-evolving cluster of weighted 1 N0 point distribution (“particles”, see figure 2.3). Starting with an initial size-N0 cluster with positions tx0, ..., x0 u approximating P0 “ Ppx0q.

N0 0 0 Ppx0q “ wj pj px0q (2.8) j“1 ÿ The particle cluster is typically propagated between sampling periods via the dynamical model such that the j j individual particle transitions xi Ñ xi`1 are randomly sampled from the dynamical model probability density function Φpxi`1|xiq and then re-weighted via the probability mass re-distributing eﬀect of the measurement likelihood Λpzi|xiq factor in equation 2.1:

Ni`1 i i Ppxi`1q “ Λpzi|xî`1qwj pjpxî`1q (2.9) j“1 ÿ j , where Ni`1 is the cluster size at time ti`1, xi`1 is the stochastically propagated position for particle j from its j i j position xi at time ti and the xi`1-marginal density of pjpxî`1q is equal to δpxi`1 ´ xi`1q

The particle approximation to the solution of 2.1 is efficient when the models are accurate and the choice of swarm size Ni at time ti is appropriate. This is because the solution form is self-similar following propagation / re-weighting, is typically a simple operation performed on the individual particles, and can handle non-linear propagations / measurement models without difficulty. Some methods vary the swarm size to optimise the filter in terms of computation time and accuracy [23, 36, 12]. Since point particles have no spatial extent, PFs are also excellent at integrating road constraints, because the point-distributions are unaffected by the complex shape of the roads compared with non-point distributions which become distorted by the road geometry/topology [22, 7]. This makes them a popular choice in low-cost urban vehicle positioning. Cluster size management is a challenge because of “degeneracy”, which occurs when many particles have very low weights, leading to biases and inaccurate solutions to equation 2.1, which may be measured by the “effective cluster size” Np [41, 20]. To solve this, particle clusters may be “re-sampled” to ensure a maximally efficient posterior distribution of particle weights and locations, though an obvious best solution does not exist. Examples of particle filters used in positioning include [21, 16, 41, 2, 36, 22, 7, 20].

28 Figure 2.3: Visual depiction of how a particle ﬁlter approximates a complex probability distribution (e.g. the ﬁlter equation 2.1) with a point cluster. In vehicle positioning, the points may move around a road map evolving the vehicle’s position probability. Image sourced from [22].

Gaussian Cluster Methods The Gaussian cluster method is similar to the PF (see above), but the individual “particles” are replaced with Gaussian distributions with pre-set parameters. This results in a reduced incidence of degeneracy as with the particle ﬁlter due to the greater sampling of the road network, and often greater speed because the spatial extent means fewer cluster components are needed than PF particles to accurately approximate the solution to equation 2.1. At each time iteration the Gaussian cluster components are propagated and re-weighted in a similar manner to the PF via dynamical propagation and measurement likelihood weighting (product terms in equation 2.1). An example form for an initialised GCM is given by equation 2.10. Some challenges are also introduced when the cluster components have spatial extent, as is the case for GCMs. In contrast with the PF, linear-Gaussian dynamical and measurement models (or linearised forms of non-linear models) are required in order to maintain the Gaussian cluster posterior-approximation structure over time, since Gaussian distributions, unlike point-masses, can become distorted with non-Gaussian multipliers. The GCM is

29 also less well suited to integrating road maps because the road bends and intersections can distort the space-ﬁlling Gaussian components at corner points [48, 31]. There is no clearly optimal method to address these challenges in the literature. The initial Gaussian Cluster (”prior”) is given by

N0 0 0 j j Ppx0q “ wj Nj px0; µ0, Σ0q (2.10) j“1 ÿ j j th , where µ0 and Σ0 are the mean and covariance of the j Gaussian component at time t0. In some GCM variants, components may be eliminated if they fall below a certain weight, or divided/split/merged according to the individual algorithm speciﬁcations as deemed optimal for speed and/or error-minimisation [11, 13, 48]. Others prefer, typically for computation-speed reasons, to reduce the Gaussian sum to a single component at the conclusion of the iteration cycle [13, 31, 54]. The method of generating a state estimate xˆi from the cluster also varies between methods. Examples of GCM methods used in positioning include [13, 12, 48, 11, 54, 6, 32].

Figure 2.4: Schematic diagram of some road map segments with a Gaussian Cluster approximate solution to the ﬁlter equation solution Pipxi|Ziq overlayed, displayed as ellipses. The position estimate may be the weighted mean of the Gaussian components. Image sourced from [48].

30 Multiple-model filters The Interacting multiple models filter (IMMF) is an estimation method for positioning designed to integrate a more sophisticated state evolution (dynamics) model for the vehicle. In motion-unsteady environments, simple dynamical models such as a linear-Gaussian model may not correctly anticipate changes in vehicle motion type and lead to estimation bias. As Lin and Bar Shalom observe, “A high gain (high bandwidth) filter is needed to respond fast enough to the platform manoeuvres while a low gain filter is necessary to reduce effectively the estimation errors during the uniform (non-manoeuvring or quiescent) motion periods. However, the Kalman Filter can only be a compromise of these two conflicting requirements” [34]. The IMMF models the vehicle motion as a set of K complementary motion states such as cruising, manoeuvring, accelerating or stopped, which toggle over time. The IMMF generally uses a time-evolving Gaussian cluster with K components, where K is the number of motion states modeled, to approximate the solution to the filter equation 2.1. The IMMF initialises the posterior P0 as a single-Gaussian or Gaussian cluster form, where each component is labelled with a dynamical mode index (DMI) corresponding to a discrete motion state. This is propagated like a GCM (see above) but each component is propagated with a different model corresponding to its DMI. For example, in the protocol originally described by Blom and Bar Shalom (1988) [6], the evolution process involves a “splitting” or “mixing” step, and a “merging” or “matching” step [30, 2]. During splitting/mixing, each component with DMI i splits into a sub-cluster of size N, with each sub-component labelled with new DMI j j N j (referred to as a “mode switch”) and multiplied by normalised weights wi , ..., wi , ..., wi , where N is the total j number of modes [6]. The weights are determined by the Markov switching matrix Tij “ wi since the changes j in motion state are typically modeled as a first-order Markov process with probabilities equal wi . During the “merge/mixing” step, the components with matching DMIs (following measurement update) can be combined into a single component with the representative DMI (the mixing protocol varies between algorithms).

The variable motion-state dynamical model structure is effective at anticipating complex vehicle manoeuvres which can reduce bias errors during events such as turns, or stop/start motion at intersections or dense traffic. However the problems inherent to regular GCMs exist for IMMFs (see “Gaussian Cluster Filters” section above), and in addition the default number of components K for an IMMF is less than the swarm size of a GCM, leading to less accurate approximate solutions to equation 2.1 and therefore bias errors. No clear solution to these challenges has been identified. Examples of IMMFs used in positioning include [6, 30, 12, 11, 9, 2, 9, 34].

2.2.2 Standard methods for integrating road maps into the position estimation process Many low-cost positioning methods integrate road constraints with GNSS in order to adjust the estimates to improve their accuracy, as well as rationalise estimates with the map to increase interpretability of output for the user. All of the common methods discussed in section 2.2.1, and others, can be (and are) in practice frequently adapted to integrate road constraints [56, 42]. Most of the highest performing low-cost positioning methods involve both filtering with GNSS in combination with road constraint integration, and perform typically better than the equivalent method without road constraints (see section 2.3.1 for a quantitative analysis) [42, 56]. There is no single method of integrating road constraints, but a variety of mathematical approaches. Due to the complexity of the road map geometry and topology, there is no clearly most efficient and precise mathematical method of integration, as GNSS and road maps have some inherently conflicting properties (continuous vs discrete, symmetric vs asymmetric, circular/spherical vs rectangular). This literature survey, as well many other reviews, show that the method of road constraint integration selected has a significant impact on the overall positioning performance in terms of accuracy and continuity of estimates [45, 42, 56]. Road maps can in turn increase, not affect, or in some cases decrease positioning accuracy compared to the same method without road maps. It is important to understand the existing types of road map integration methods in order to identify trends in performance, determine the status of the present research into low-cost positioning and guide the development of an optimised method of integrating road maps with GNSS (chapter 4). In the literature survey, it was observed

31 that the methods of road constraint integration tend to fall into two broad classes.

Road constraint integration method 1: prior state-unconstrained (PSU) methods One of the two main methodological categories identified for integrating road constraints into low-cost positioning which will be referred to as Prior State-Unconstrained filtering methods (PSUs). The main characteristic of PSUs is that they generate position estimates using GNSS data using a filtering method (e.g. KF, PF, GCM) prior to considering road constraints, which are then used following the filter step as a correction to the constraint-free position estimate (“rationalisation”) [42, 56, 28]. PSUs were a predominant positioning method in the late 1980s and 1990s because they have relatively low computational burden (filter followed by a “map match” step) compared with other methodologies for integrating road constraints [42, 56]. The map match is often a search algorithm which scans the road network GIS database for candidate on-road positions and selects one that optimises an objective function (e.g. probabilistic likelihood, or distance from the filtered point-estimate to the map point) [56, 42, 45]. It was found that PSUs tend to correlate with methods that are classed as “map matching” methods in the literature, though not exclusively. This survey also found that PSUs can be further divided into two broad classes (“geometric” and “topological”) based on which GIS information was used in the map match step. Geometric methods generally use only geometric map information about the road map, i.e. the absolute positions of the map points, as the input to the map match algorithm. The objective function used to select the optimum match is often a Euclidean distance-related metric, e.g. the absolute or weighted distance from the map point to the filtered estimate which is then minimised to obtain an optimal on-road estimate of the vehicle’s position (see figure 2.5 for a visualised example). If likelihood is the objective function, then the map point with greatest likelihood (e.g. based on the posterior distribution of the filtered position) may be chosen. This is typically similar to minimising a weighted distance from the map points to the filtered estimate, related to how closely the map point fits within the KF elliptical confidence region. Geometric PSU positioning methods tend to perform well when SatVis is high, and helps to make output interpretable to the user by overlaying estimates on the road map. However it may perform poorly when SatVis is low and especially in regions of high road density (intersections/blocks with lanes), exhibiting jerky or discontinuous estimated on-road positions. This is because the underlying filtered estimates may drift across the map, causing the estimate to jump between roads when a distance-type objective function is used [42, 28, 56, 51, 50]. Some more recent PSU algorithms use an on-board camera and/or LIDAR to detect lane markings in order to improve estimates of lateral position adding further precision to position estimation [55, 24, 49]. While evidence was presented to show improvements in lateral displacement error (i.e. cross-road position), a common methodological shortcoming in the self-evaluations was that the test paths did not include turns at intersections, but rather a pre-measured singe-road test path, which does not test the algorithms’ ability to identify the difference between turning onto a different road and merely changing lanes on the same road. Another shortcoming of such methods is that lane markings are not universal features of urban roads. It is also unclear how well the algorithms would work in a prolonged GNSS outage over a street change (this was an untested scenario in all of the above papers) given that lateral positional aid cannot necessarily improve along-road position estimation or street segment identification, though this may be somewhat helped if the map-matching method was instead prior state constrained (or PSC, see next subsection ”Road constraint integration methods: prior state-constrained filters (PSC methods)” for further discussion). Furthermore, LIDAR is not a low-cost technology so does not present a viable solution to the research problem being considered. Other recent PSU algorithms use on-board cameras to detect landmarks and visual features and attempt to match them with registered landmarks with known positions to localise the vehicle [35, 40]. While this is a promising approach, the processing time for images and landmark registry search is still a challenge as indicated in the concluding remarks of [35]. Such algorithms also add less value in areas where prominent / detectable landmarks do not exist or are not registered in databases. The algorithm in [40] focuses on lane markings and matching with satellite images of lane markings, in an attempt to match observed markings with known positions of markings in satellite images. This presents some pattern recognition challenges in that, as indicated in the self-evaluation, the algorithm is at risk in falling into local-optimum rather than global-optimum solutions when patterns matching results in false positives (incorrect matches), which may incorrectly position the

32 vehicle for prolonged periods (e.g. on the wrong road). A common thread appears of such recent methods appears to be on reliance on lane markings or registered landmarks which cannot at present produce a viable solution to the low-cost vehicle positioning problem in all road enviornments.

Topological methods generally use both geometric and topological information about the road map as an input into the map match algorithm. Topological information includes the connectivity of the road segment network, i.e. which segments are neighbouring, which may be captured based on an adjacency matrix if the road network is mathematically described as a graph with nodes at map points and road segments as vertices. The objective function will include a combination of a distance-type minimisation between road map points and the ﬁltered estimate, but assign a penalty to road points that are on segments that do not connect with the vehicle’s previous segment, since this positional transition is not feasible [42, 43, 50]. This more sophisticated objective function tends to smooth estimation in low SatVis environments even if the ﬁlter drifts, however may not perform well if the wrong road segment is initially estimated [42], since the true segment may not connect to the estimated segments.

Figure 2.5: Schematic of how a PSU method might match an oﬀ-road (constraint-free) probability distribution of position (wide ellipse) to the road map via a lateral projection, so that the output is on-road. Image sourced from [28]

33 Examples of PSU methods include [51, 28, 33, 7, 39, 60] (geometric methods) and [43, 50, 37, 10, 59] (topological methods).

Road constraint integration methods: prior state-constrained filters (PSC methods) Another class of methods for integrating road constraints into low-cost positioning identified in the literature will be referred to as Prior State-Constrained filters (PSCs). These methods use filtering-type methods to treat the GNSS inputs, however the filtering method uses a state space that is constrained by the positions on the road map, rather than using a two-stage process of state-unconstrained filter + map rationalisation (see figure 2.6 for an example positional probability over the constrained state space). This is typically achieved by constraining the state evolution model of the filter Φpxi`1|xiq (a component of equation 2.1) to only allowing physically feasible transitions through the road network along the road segments. This more complex evolution model, when combined with the GNSS measurement model, results in more complex solutions to the filter equation 2.1 which increases computational burden significantly [12, 31, 48]. However, this method integrates the geometry and topology of the road network without the need for an additional matching step, eliminating any potential errors or modeling flaws by reducing the PSU’s two-step process into one step. The PSCs’ more complex solution to equation 2.1 tends to be a more accurate approximation to the true solution, and therefore tends to have greater accuracy, continuity and has better performance and recovery speed in low SatVis and other positioning-hostile environments than PSUs (see section 2.3.1 for a quantitative analysis) [42, 23, 13, 12]. Examples of PSCs in the literature include [13, 31, 54, 11, 36, 22, 20, 34].

34 Figure 2.6: Visualisation of a PSC estimation method, where the probabilistic solution Pipxi|Ziq (blue) to the ﬁlter equation 2.1 is road constrained. The distribution is constrained to the road and moves along it as time elapses. Image sourced from [23].

2.3 A quantitative evaluation of the performance of existing positioning solutions

This section contains the quantitative results of a survey of existing low-cost positioning algorithms. A key aim is to determine the present state of the research and what shortfalls still exist, and how performance correlates with particular features of the algorithm design in order to better inform the proposed solution in chapter 4. As foreshadowed in the introduction, other important aims are 1) an empirical study of how positioning-hostile environments aﬀect low-cost positioning performance in section 2.3.1 and 2) what quality issues exist in the performance self-evaluations of existing algorithms, discussed in section 2.3.2. Over 50 vehicle positioning algorithms were examined as the body of the survey, from a period from 1986 to the present but with a heavy emphasis on more current (last 10-15 years), widely used and/or those employing more statistically robust or realistic models. Several survey papers were also examined for further insight into existing

35 literature, including [42, 57, 56, 45, 46].

2.3.1 Quantitative positioning performance survey across a range of positioning environments The results of this survey summarise low-cost positioning algorithm performance self-evaluations based on real data from the original primary sources. The literature surveyed encompassed a range of positioning methods including various approaches to integrating road contraints (both PSC and PSU), ﬁltering algorithms, as well as a range of positioning environments (friendly and hostile). This section is sub-divided into performance results factored by the various positioning environments to understand and compare impacts on performance. Note that, unless otherwise stated, all methods evaluated use ﬁltering, allowing positions to be estimated at any level of satellite visibility, not only at SatVisě4.

Performance survey design and results - positioning friendly environments Over 35 papers surveyed included performance evaluation results of positioning algorithms in positioning-friendly environments. This includes environments that do not have a particularly high density of roads, and have moderate/good (SatVisą3) GNSS conditions across the large majority of the datasets. Methods evaluated used GNSS signals (either SPP or raw code-based psedudoranges) and some integrated road maps. Around a third of these papers evaluated the performance of GNSS positioning methods that did not integrate road constraints. The remainder did integrate road maps into positioning, including a balance of PSU and PSC algorithms to allow a representative comparison of the main classes of low-cost positioning methods. The survey results showed unanimous positioning performance improvements due to the integration of road maps, over a range of metrics including average positioning error as well as peak-error magnitude, duration and error divergence/growth rate [14, 42, 7, 3, 13, 37]. The magnitude of error reduction (the difference in average error between methods integrating/not integrating road maps) varied according to factors such as the SatVis level and the method of GNSS/road constraint fusion (PSC or PSU). Accuracy was typically of the order of 10-25m in constraint- free positioning which reduced to 10m or sub-10m order accuracy following integration of road constraints [42]. The percentage-reduction of average positioning error between methods integrating/not integrating road maps ranged from 30% to 87%, with an average of 66%. These results are summarised in figures 2.8. When the method of road constraint integration was factored into the evaluation, a substantial difference was observed between PSU and PSC constraint-integration approaches. PSU methods averaged 55% error reduction from average error levels in constraint-free positioning (when SatVis was roughly 4) compared with a corresponding 72% error reduction in the case of PSC methods. This correlated with a clear difference in the frequency of high- error events in moderate GNSS-conditions, with PSU methods achieving “significant” error levels (10m or more) with 31% greater frequency in moderate GNSS conditions than PSCs. Examples where this performance difference between PSCs and PSUs was greatest included at intersections, during turns or sudden stops/accelerations, and service smoothing during occasional medium GNSS SatVis reductions. “Jumps” between road segments due to segment-mismatch were observed to be significantly less frequent in PSC algorithms as hypothesised in section 2.2.2 [37, 57, 42]. These results are summarised in figure 2.8.

The error-reducing eﬀect of road constraints in urban canyon environments The second part of the quantitative literature survey investigated the impacts of road constraint integration on positioning performance in urban canyon (low SatVis) environments. This corresponds to environments where “GNSS outages” (SatVisă4) occur frequently and over extended durations and where multipath may be present. Outages with SatVis=2 or 3 will be referred to as “partial” outages and SatVis=0 or 1 as “severe” outages. More than 20 papers surveyed included positioning algorithms evaluated in urban canyon environments including the full list of those cited in sections 2.2.1 and 2.2.2. A roughly equal balance of of PSUs and PSCs were included in this sample to investigate the performance impacts of constraint integration methods in urban canyons.

36 It was found that across the board (high SatVis, partial and severe GNSS outage conditions), road constraint integration improved positioning performance in terms of reducing errors and containing error growth compared with constraint-free methods. Partial GNSS outage refers to 0ăSatVisă4, whereas GNSS outage refers to Satvis=0. PSCs outperformed PSUs in terms of the error reduction magnitude and error growth rates to a signiﬁcant degree. In SatVisď2 environments, PSCs reduced average error from the constraint-free case by 75% compared to 31% for PSUs, a much greater discrepancy than in positioning-friendly environments. It was observed that PSCs contained positioning error growth during severe GNSS outages and even maintained lengthy periods of stable error of less than 20m. PSUs typically could not contain positioning error growth, with error often growing until a high “plateau”, then dropping following an increase in SatVis. The level of average error-reduction from constraint-free methods to PSCs was higher by 36% when SatVis was reduced from partial to severe GNSS outage levels, compared to a decrease of 25% in the case of PSUs [42, 13, 12]. This means that the highest utility of road map integration was gained by PSCs in severe GNSS outages, with PSUs not obtaining as much utility from the map data. No PSU method tested was able to provide better than 25m errors over moderate length (30 second) GNSS outages, while PSC methods averaged 13m errors in similar environments and SatVis levels.

Positioning performance in regions of complex road network topology Part of the low-cost positioning algorithm performance survey focussed on positioning in the presence of hostile factors that were not related to GNSS signals. This included topologically complex network regions (see section 2.1.1 for overview), with over 20 papers surveyed in this category, including all those cited in section 2.2.2. A balance of PSU and PSC algorithms were performance evaluated and compared with road constraint free methods. Overall, positioning in this environment was observed to increase the risk and duration of network segment mismatch as the leading source of positioning error in both PSU and PSC methods [57, 13, 42, 56]. The average positioning error increased from 9m to 12m for PSCs and from 12m to 20m for PSUs as the vehicle moved through topologically complex network from less complex regions (summarised in ﬁgure 2.7).

Figure 2.7: Performance results summary of low-cost positioning methods with road constraints based on aggregating performance tests found in this literature survey. Average errors shown by road constraint integration method and level of road topological complexity.

37 The larger increase in average positioning error (and corresponding service performance decrease) in PSUs (8m vs 4m, or 67% vs 44%) is due to the occasionally severe cases of trajectory mismatch which were observed to occur more frequently in PSU methods. In some instances it was observed that integrating road constraints increased positioning error relative to constraint-free positioning in PSU methods (but not PSC methods), leading to jumps between road segments/blocks often with long recovery delays (5+ seconds) and frequently diverging to above 20m. PSU and PSC positioning algorithms both demonstrated improved performance compared with constraint-free methods in topologically complex areas. The observed relative improvement was 74% in the PSC case and 40% in the PSU case (summarised in ﬁgure 2.8).

Figure 2.8: Performance results of the low-cost positioning method literature survey, with performance indicators presented obtained by aggregating performance data in the literature. Average percentage error-reduction when road constraints are integrated compared with constraint-free. Results factored by method of constraint integration, and positioning environment (SatVis and topological complexity).

Performance impacts of non-uniform vehicle motion and dynamical model parameters The final part of the low-cost positioning algorithm evaluation survey investigated the impacts of vehicle motion state change. Twenty four papers surveyed analysed positioning output in environments where the vehicle motion state was unsteady, unpredictable or non-uniform. This included sharp turns, sudden stops/accelerations or combinations in quick succession. The papers represented both PSC and PSU methods (a total 15 and 11 tests within the 24 papers respectively). The survey also investigated the impact of the state evolution model parameters on positioning performance in unsteady motion environments, with 9 papers being classified by this survey as having “sophisticated” dynamical models, defined as having a more complex parameterisation than a linear-Gaussian state evolution model with time-invariant model parameters (e.g. equation 2.3). Sophisticated models included, for example, interacting multiple model varieties, or models that had additional dynamical state parameters such as accelerations or turning rates [26, 29, 6, 53].

The summary of the performance survey was that sophisticated algorithms showed 79% error reduction from

38 unconstrained GNSS when integrating road constraints, vs 69% for simple methods [34, 30, 12, 11, 16, 41]. PSC methods with sophisticated dynamics (which included multiple-hypothesis methods such as particle ﬁlters and Gaussian cluster methods) reduced the incidence of trajectory mismatch at intersections during turns and trajectory changes compared with PSU methods [48, 13, 42, 31]. In unsteady motion and moderate GNSS visibility conditions, the error reduction compared with constraint-free positioning was 40% for PSC methods, and 25% for PSU methods. In GNSS-scarce conditions (SatVisď2), the corresponding ﬁgures were 75% and 13%. It was also found that within PSC methods, those with multi-component representations such as GCMs and PFs performed better if periodic cluster size reduction was performed. This may involve trimming components with low weights to reduce bias errors and provide better anticipation of unsteady motion through intersections and areas where choices could be made about trajectory.

2.3.2 Some remarks on the reliability of self-evaluation experiments It is clear from the performance survey of low-cost positioning solutions that further work is necessary to achieve continuity and accuracy in all common positioning environments, particularly urban canyons. However, also identified was a risk that the positioning algorithm performance self-evaluations in the literature may tend to over-estimate performance, and understate flaws and risks. It is important to highlight these issues in order to better gauge the present state of the research, and guide a more robust testing methodology for the new proposed algorithm (see chapter 5). A key issue identified in the survey that was also highlighted in other surveys was that the scope of test parameters was often narrow, and thus conclusions drawn from these evaluations were often too broad. Namely, potential weaknesses in algorithms were often not robustly examined or identified with appropriate test settings and performance claims were often extrapolated beyond the scope of presented test results.

These observation have been supported by existing literature reviews. According to Quddus et al, “Very few existing map-matching algorithms provide a meaningful validation technique”, a sentiment shared by White et al who claim in their performance review of map-matching algorithms that “more attention needs to be given to how diﬀerent algorithms can be compared empirically” [42, 56]. White suggested also that “algorithms need to be evaluated on a wider array of [urban road network] routes”, that “it is important to consider truly urban routes because of the problems that arise in urban canyons”, and that “more attention needs to be given to the problems that arise at intersections”. In his summary, he notes that “the limited number and variety of routes considered here obviously prevent us from drawing any real conclusions” [56]. Quddus et al demanded “[better] performance evaluation, especially in dense urban areas, and development of conﬁdence indicators”.

2.3.3 Conclusions In this chapter a review was undertaken of the literature covering low-cost positioning algorithms using (at most) GNSS and road maps. Over 50 algorithms were examined and compared based on their performance self-evaluation data, to understand the present state of the research. A range of different methods were surveyed covering a broad range of positioning environments and test parameters, to generate some understanding on how these factors impacted on positioning accuracy and continuity. The results of this survey are presented in section 2.3.1, and are broader and more current than other low-cost positioning algorithm surveys found in the literature. A key result is that, road constraint integration was found to have a consistently positive effect on positioning performance in terms of accuracy and continuity in all positioning environments. Additionally, the method of road map integration was a significant factor in predicting performance, with the accuracy of PSC methods ranging from 30% to 80% better than PSU methods in positioning-hostile environments. Another key finding was that the road constraints have the biggest positive impact on positioning performance in positioning hostile environments, such as urban canyons and topologically complex areas, when compared with constraint-free methods. The state of present research identified from these results show that further work needs to be done to create an accurate and reliable low-cost positioning solution in all common positioning environments, particularly low SatVis scenarios,

39 where errors often exceed 10m and diverge over time instead of stabilising. The survey has also highlighted the need to better understand the impacts of the vehicle’s positioning environment on positioning performance, as this was found to greatly aﬀect performance in ways which have not been systematically studied. In chapter 3, theoretical evaluation of the performance impacts of the positioning environment on accuracy and continuity will be undertaken. This will also aim to measure impact of road constraint integration in these environments in a statistically signiﬁcant way. The results of chapter 3 together with this chapter’s performance survey will form the evidence base for a new positioning algorithm developed in chapter 4 and the testing methodology in chapter 5.

40 Chapter 3

Quantifying Low-Cost Positioning Performance Impacts of Vehicular Environments

3.1 Introduction

Understanding the nature of performance fluctuations of existing low-cost vehicle positioning solutions as the vehicle traverses an urban environment is important for targeting improvements to identified weaknesses. This includes understanding the key drivers of this performance variability, and how factors external to the vehicle (e.g. urban canyons, traffic conditions - namely the “positioning environment” introduced in chapter 2) can influence the accuracy and continuity of positioning solutions [42, 26, 9, 7]. The key objective of this chapter is to quantify the impacts of some common positioning environments on low-cost solutions by developing a theoretical statistical model for positioning error and evaluating over a range of parameters that simulate positioning environments. The aim of the experiments is to create a performance evaluation reference dataset for low cost positioning that will 1) measure the impact of positioning environment on low cost positioning performance and 2) measure the impact of integrating road constraints into the positioning, and how this impact varies across the positioning environments. Such an analysis of positioning environment has, as far as is known, not been performed and constitutes an important gap in the literature [42]. This dataset would be an important step toward better understanding positioning error characteristics in existing positioning solutions, and diagnosing error spikes/divergences in a more theoretically sound way. It would also allow for the ranking of the environmental, as well as the algorithmic, factors that lead to significant positioning error, so that a robust approach may be taken to optimising low-cost positioning across all environments in a targeted way. The performance data in the literature are overwhelmingly based on algorithmic self-evaluations that often have inconsistent positioning performance measurement methods, varying scope and test parameter variations which do not allow for robust or statistically significant aggregation into a dataset such as the one created in this chapter [42, 58, 23]. The data will then directly inform the design of the original low-cost positioning algorithm in chapter 4.

3.2 Methodology for measuring impacts of the vehicle’s environment on low-cost positioning performance

The chosen method for measuring impacts of positioning environment on low-cost positioning performance was by evaluating the positioning error of a low-cost ﬁlter positioning a vehicle in a range of positioning environments, and thus generating a performance impact dataset for each environment. It was decided that the optimal approach

41 was to evaluate the performance of the ﬁlter based on a theoretical “expected positioning error” calculation across a range of mathematically modeled positioning environments (the Bayesian Root-Mean-Square Error - BRMSE), see sections 3.2.1 and 3.2.2. The calculation is equivalent to synthetically generating a large quantity of GNSS measurement/trajectory data of a vehicle through a road network and weighted-averaging the estimation error results over the simulated data, weighted by the modeled likelihood of the trajectory/GNSS measurements (see section 3.2.1 for details). The data that are synthetically generated in order to produce the environmental impact dataset include: 1. The road network and the positioning environment of the vehicle in that road network 2. The vehicle’s trajectory through the road network in a particular environment as a series of synthetically- generated states px1, x2, ..., xkq, with corresponding positions px1, x2, ..., xkq

3. GNSS measurements pz1, z2, ..., zkq based on the vehicle’s simulated trajectory and established measurement models

4. The Kalman Filter estimates of the simulated trajectory pxˆ1, xˆ2, ..., xˆkq computed from the measurements pz1, z2, ..., zkq The synthetic data generation model is designed to produce data that measures the impact of a number of discrete environmental factors (including urban canyons, the vehicle’s state of motion, and road network geometry) that combine in various permutations to generate the “positioning environment” (see section 3.2.2 for details of this model). The measurements include the impacts of the factors in isolation as well as in combination, and also performance-evaluate low-cost methods that integrate road constraints, and compare them to those that do not. The data generation models are based on empirically validated models in the literature [42, 12, 23].

3.2.1 The Bayesian Root-Mean-Square Error (BRMSE) measure of positioning performance in simulated environments Overview of the impact measure Several candidate measures were considered for the impact assessment of positioning environment on low-cost positioning performance (see next section for feasibility evaluation of candidate measures). It was decided that the optimal choice was the Bayesian Root-Mean-Square Error (BRMSE), deﬁned as below

RMS MS i ” i (3.1) is the BRMSE at time ti, where b

MS 1 T i “ XiPS pxî ´ xiq pxî ´ xiq ΛpZi|Xiq ΠpXiq dpX,Zq (3.2) żX,Z and where X is the set of allowed on-road positions, Z is the set of possible GNSS measurements, ΛpZi|Xiq is the likelihood of measurement set Zi “ pz1, ..., ziq given a simulated trajectory Xi “ px1, ..., xiq up to time ti, with likelihood ΠpXiq of being generated. RMS i is evaluated at each sampling time ti generating a time series for every individual set of environmental parameters which feed into equation 3.2, see section 3.2.2 for details. These time series may be used to generate RMS environmental impact metrics e.g. the maximum error over an environment, the rate of i increase, time taken RMS to recover from an error growth event or simply an average error over a particular period. The i time-series based metrics may be compared with metrics computed in similar ways from different environments in order to directly measure the differential impact due to environmental factor differences (the environmental impact). This may be used to determine which environments are more favourable/hostile to positioning and by what amount. It should be noted that all instances of the term “error” appearing in this chapter in reference to the BRMSE results should be interpreted as “theoretical means-square error”.

42 Interpretation and justiﬁcation of the BRMSE measure to measure environmental impact on low-cost positioning

MS For a given environment, i represents an expected squared positioning error at time ti, which determines the 2 expected value of the squared difference pxi ´ xîq between the true simulated position xi and the KF estimate computed from the GNSS measurement zi generated by the measurement model Λ. The measurement model is 2 a stochastic model conditional on the true position xi. The expected value is found by then averaging pxi ´ xîq over all possible simulated vehicle trajectories Xi through the simulated network, and over all the associated GNSS measurements Zi generated from those positions which produce the estimatesx î, weighted by the respective MS likelihoods based on the models ΛpZi|Xiq and ΠpXiq. The i measure therefore accounts for random noise in the GNSS measurements and a range of possible vehicle trajectories, adding robustness to natural variations through this weighting. This is equivalent to, for a given environment, running a Monte Carlo simulation with generating functions ΛpZi|Xiq and ΠpXiq for the random trajectories and random GNSS measurements, and computing the average estimation errors at various times from a low-cost filter from an infinite number of random trials. The various positioning environments can be simulated by varying the input parameters to equation 3.2. This includes

1. Whether road constraints were integrated into positioning. Where they are integrated, the method of integration is a PSC method (see section 2.2.2, chapter 2 for deﬁnition) 2. The road network feature (either “straight section” or “right turn” at a corner). 3. The level of satellite visibility (SatVis) 4. Vehicle motion state (either cruising over the full journey duration, or including a period of stoppage involving either gradual or rapid deceleration/acceleration).

See section 3.2.2 for further details on how these factors are simulated. Some alternative environmental performance impact measures were considered and included: Option 1) Measures based on real data collected in original experiments. This would include collecting raw vehicle position data z1, z2, ..., zk from a GNSS-receiver from a series of vehicle journeys through a road network to sample the desired set of positioning environments and computing the positioning error |xî ´ xi| based on an appropriate choice of ground-truth xi. This was declined due to the lack of control over environmental parameters in practice, e.g. traffic conditions, desired satellite visibilities over desired durations etc. Other challenges related to this measure is obtaining a sample of sufficient size to be statistically significant when comparing between environments, and also obtaining ground-truth data of very high accuracy to estimate error, which cannot be achieved in all circumstances (particularly very low SatVis scenarios) [7, 42, 56].

Option 2) Measures based on low-cost positioning performance data gathered from the self-evaluations of vehicle positioning algorithms in the existing literature. Generating environmental performance impact measures would involve aggregating positioning error measurements across a broad range of experiments and papers, and focussing the search on experiments conducted in the desired environments. Whilst this may circumvent some issues around the resource intensity of collecting original data as per candidate measure 1, there are many challenges to this method which include difficulty in controlling for factors such as differences in positioning methods used / arrange- ments and types of low-cost sensors used, as well as generating a reasonable sample size in all common environments (not just those which are easier to replicate). These uncontrolled-for variations could mask any measurable impacts of differences in the positioning environment and would thus not be credible [7, 42, 56].

Option 3) Measures computed using Monte Carlo (MC) simulations to create positioning error time series. The MC could simulate vehicle trajectories, GNSS measurements and position estimation generated from applying a ﬁltering method to the latter, then diﬀerence the generated positions from estimates and average them over the set of simulations. The challenge of using this method is that, while it avoids uncontrolled variations in candidates

43 1 and 2, it does not necessarily overcome statistical significance issues because the sample size may be too small. Generating large enough sample sizes in all permutations of environmental factors may prove too computational intensive to be practicable [36, 23]. Option 4) Measures based on existing theoretical models for mathematical simulation of positioning environments, vehicle trajectories, GNSS-data and estimation methods. Option 4 circumvents both resource and data collection design issues, however was declined after examining 2 existing and widely used simulation-based error measures for feasibility: the Bayes Mean Error (BME) and Cramer-Rao Lower Bound (CRLB) [52]. These measures quantify positioning performance using stochastic models to simulate natural variations in trajectory, GNSS measurements and position estimation. The BME (see equation 3.3) computes the mean error in a similar Bayesian averaging approach to the BRMSE, however does not allow for a closed-form exact solution due to the integrand’s particular form. This precludes this measure from being feasible as computing the measure over the full range of positioning environments would require significant computation, or approximate computation which could add an uncontrolled noise into the resulting impact measure. The CRLB computes an upper-bound on the positioning performance of optimal estimators (based on a lower-bound of the estimation covariance), with a formulation specific to filtering given in [52]. This allows for a closed-form exact solution when simulating a limited subset of common environments, however not in others where the topology of the road network is not simple, such as in a turn / intersection scenario.

BME 1 T i “ XiPS pxî ´ xiq pxî ´ xiq ΛpZi|Xiq ΠpXiq dpX,Zq (3.3) żX,Z b The BRMSE measure is designed to circumvent all of the issues listed in Options 1-4. The integrand is quadratic in xi and z1, ..., zi which allows for a closed-form exact solution in all simulated environments assuming linear- Gaussian stochastic models and linear-quadratic estimation, unlike other theoretical model based measures such as the BME or CRLB where closed form solutions aren’t achievable in all environments (e.g. BRMSE can handle road constrained positioning, constraint-free positioning and non-trivial road networks). The simulation models Λ and Π in equation 3.2 are based on empirically validated observations about vehicle motion tendencies and GNSS noise, meaning there is little loss of robustness compared with real-data output but with significantly less resource intensity and much more control over sampling the desired environments. Other benefits include the guarantee of statistical significance (infinite effective Monte-Carlo sample size), no uncontrolled inconsistency in the performance metric between environments that may mask genuine impacts of environmental differences. Other masking effects in the literature may arise due to varying data sources, model parameters, estimation methods and/or ground truth sources between individual algorithm performance evaluations, however these are strictly consistent in the BRMSE RMS evaluation. The controlled simulation parameters mean all measure differences between two time series of i are direct measures of the statistically significant impact due to the environmental change between the two associated experiments.

3.2.2 Parameters and design of environmental impact data collection experiments Input parameters into equation 3.2 needed to generate the positioning error dataset for impact evaluation include the description of the positioning environment, road network, vehicle motion, GNSS measurements and estimation output.

3.2.3 Road Network Representation The road network is modeled as a zero-width connected line segments (see figures 3.1 and 3.2). The points along the line segments are the only allowed positions for the simulated trajectories (a hard road-constraint), and this is reflected in the choice of integral domain X in equation 3.2 which does not include off-road positions. Two scenarios are included in the performance impact evaluations: a simple control case (straight section of road) and a more topologically complex network, including right turn along a street corner (see figure 3.1 and figure

44 3.2). These are simulated in equation 3.2 by using the “along-road” co-ordinate xi which is then integrated over the distance traveled, with associated co-ordinate transformations applied to the integrand (this only materially impacts the constraint-free estimation case, see section 3.2.5 for further details). The choice of co-ordinates used to simulate the road network environments is northing-easting-up (NEU) for simplicity, since a convenient origin may be defined and have axes align with the road network principal directions with dimensions that complement the stochastic models chosen but describe the road map conveniently. NEU is a 3-D system of orthogonal co-ordinates with an origin set to an arbitrary point on the Earth’s surface where X is the North-bound axis, Y the East-bound axis and Z the axis perpendicular to the Earth’s tangent plane at the origin. It is not assumed that the road is flat; rather the altitude of the road is randomly generated as a random walk with every vehicle trajectory having a different random altitude profile (see section 3.2.4).

Figure 3.1: Schematic of the simple-straight-road network simulated in the BRMSE calculations of environmental positioning-performance impacts. NEU (northing-easting-up) and local-xAR (along-road distance) co-ordinate systems used in calculations are deﬁned as shown.

45 Figure 3.2: Schematic of the road-corner network simulated in the BRMSE calculations of environmental positioning-performance impacts. NEU (northing-easting-up) and local-xAR (along-road distance) co-ordinate systems used in calculations are deﬁned as shown.

3.2.4 Models for generating vehicle trajectories and GNSS measurements Input parameters into equation 3.2 are required for 2 synthetic data-generation models : one for generation of trajectories along the road network, and a second for generating GNSS measurements by adding noise to these generated trajectory positions.

Vehicle trajectory generation model A linear multiple model (MM) discrete-time dynamical model is adopted for the generation of vehicle trajectory which randomly generates the along-road positions and state vector xi in a recursive way from the previous state xi´1: m m xi “ Fi xi´1 ` wx,i (3.4)

, where xi “ pxi, vi, hi, ∆tiq is respectively the vehicle’s along-road position, along-road speed, altitude and receiver clock bias at time ti. m Fi is the linear propagator matrix at time ti and m is the motion state of the vehicle which can either be cruise, decelerating, accelerating or stopped. The parameters in equation 3.4 deﬁne the term Π in equation 3.2 because this term is deﬁned as the probability density of the state transitions as described by the stochastic model in equation 3.4. The linear propagation model Fi can vary with time and takes one of 4 forms corresponding to the states of motion, as indicated by the superscript:

46 1 δti 0 0 0 1 0 0 F 1 “ i ¨0 0 1 0˛ ˚0 0 0 1‹ ˚ ‹ m “ 2˝ corresponds to‚ “stopped” mode, with linear propagator

1 0 0 0 0 0 0 0 F 2 “ i ¨0 0 1 0˛ ˚0 0 0 1‹ ˚ ‹ m “ 3˝ corresponds‚ to the “linear accelerating” mode, with linear propagator

1 0 0 0 η 1 0 0 F 3 “ a i ¨ 0 0 1 0˛ ˚ 0 0 0 1‹ ˚ ‹ and m˝“ 4 corresponds‚ to the “linear decelerating” mode, with linear propagator

1 ηd 0 0 0 η 0 0 F 4 “ d i ¨0 0 1 0˛ ˚0 0 0 1‹ ˚ ‹ , where δt˝i “ ti´ti´1 and‚ηa and ηd are input parameter that control the magnitude of linear acceleration/deceleration. m The process noise wx,i is zero-mean and time-varying in its intensity as a function of dynamical mode index: 0 0 0 0 0 0 0 0 2 1 0 σv 0 0 2 0 0 0 0 Qi “ ¨ 2 ˛ and Qi “ ¨ ˛ 0 0 σh 0 0 0 0 0 ˚0 0 0 σ2 ‹ ˚0 0 0 σ2 ‹ ˚ ∆t‹ ˚ ∆t‹ 3 ˝ 4 ‚ 1 ˝ ‚ Qi and Qi are both equal to Qi . If the simulated environment is steady vehicle cruise motion, the m “ 1 model is used over the full trial duration. If a stoppage is induced to simulate a more complex positioning environment with motion state variation, then the trajectory generation model will change to include a stoppage by toggling from cruise -ą deceleration -ą stopped -ą acceleration -ą cruise matrices based on pre-speciﬁed transition times and acceleration/deceleration magnitudes.

GNSS measurement generation model

Measurements zi are generated by converting the generated “true” trajectory into GNSS pseudorange measurements and adding noise, using a linearised model:

L pzi ´ z0,iq “ Hi pxi ´ xi q ` wz,i (3.5) , where

L • xi “ p0, 0, 0, 0q is the static linearisation point • z ρ0 , ..., ρ0 0,i “ p i,1 i,ni q

0 L • ρi,j “ |Xi,j ´ xi |

47 • Xi,j “ pXi,j,Yi,j,Zi,jq is the position of satellite j at time ti in NEU co-ordinates

• wz,i is the additive zero-mean white-noise with potentially time-varying covariance Ri

Hi is the matrix of partial derivatives of the non-linear pseudorange measurement model (equation 3.6) with respect to the state variables px, v, h, ∆tq for each satellite j at time ti, evaluated at the linearisation point xL. This is equal to

0 0 p´Xi,1q{ρi,1 0 p´Zi,1q{ρi,1 1 . . . . Hi “ ¨ . . . .˛ 0 0 p´Xi,n q{ρ 0 p´Zi,n q{ρ 1 ˚ i i,ni i i,ni ‹ ˝ ‚ . Where there is a road corner, the model changes after the corner has been traversed to

0 0 pÝi,1q{ρi,1 0 p´Zi,1q{ρi,1 1 . . . . Hi “ ¨ . . . .˛ 0 0 pÝi,n q{ρ 0 p´Zi,n q{ρ 1 ˚ i i,ni i i,ni ‹ ˝ ‚ since the along-road direction is along the y NEU axis after the corner (see figure 3.2) The pseudorange measurement model is given by

2 2 2 ρi,j “ pXi,j ´ xiq ` pYi,j ´ yiq ` pZi,j ´ hiq ` c ∆ti (3.6) with error covariance matrix b

R diag σ2 sin θ0 , ..., σ2 sin θ0 (3.7) i “ p z { i,1 z { i,ni q 0 where pxi, yi, hiq are the NEU co-ordinates of the vehicle’s receiver and θi,j is the angle of elevation to satellite j from the linearisation point.

3.2.5 Estimation Model Co-ordinates and input Parameters

The position estimatex î in equation 3.2 is the output from a Kalman filter (KF) that takes the generated GNSS measurements zi as measurement inputs. The performance tests compare a Kalman filter which integrates the road map as a constraint into positioning, and one which ignores the road map data as a control case. ˆ The KF estimates the true state vector of the vehicle xi “ pxi, vi, hi, ∆tiq with an estimator xˆ “ pxî, vî, hi, ∆ˆtiq. If road constraints are integrated into positioning, xî is 4-dimensional, if not then it is 6-dimensional since then there is also a perpendicular-to-road component for each estimate. In the constraint-free KF case, the state vec- ˆ tor estimated is xi “ pxi, yi, vx,i, vy,i, hi, ∆tiq, where the estimate is denoted xˆ “ pxî, yî, vˆx,i, vˆy,i, hi, ∆ˆtiq, where pxi, yi, hiq is the NEU vehicle position and pvx,i, vy,iq is the along-ground-plane speed. The estimation error of the 2 2 constraint-free KF is equal to pxî ´ xiq ` pyî ´ yiq . a The KF is initialised at x0 “ p0, v0, h0, 0q in the constraint-integrated estimation case. In the constraint-free x y estimation case, x0 “ p0, 0, v0 , v0 , h0, 0q.

The time-invariant linearised dynamical model used in both road constrained and constraint-free KFs is

48 xi “ Fi xi´1 ` wx,i (3.8) with only one motion state (a “naive” motion model). In the road-constrained case :

1 δti 0 0 0 1 0 0 Fˆ “ i ¨0 0 1 0˛ ˚0 0 0 1‹ ˚ ‹ ˝ ‚ 0 0 0 0 2 ˆ 0σ ˆv 0 0 Qi “ ¨ 2 ˛ 0 0σ ˆh 0 ˚0 0 0σ ˆ2 ‹ ˚ ∆t‹ , where the˝ estimation dynamical‚ model noise intensities are assumed to be correctly speciﬁed (equal to the gener- 2 2 ating model), i.e.σ ˆ‹ “ σ‹. The road-constrained measurement model is :

ˆ L pzi ´ z0,iq “ Hi pxi ´ xi q ` wz,i (3.9) and ˆ 2 2 Ri “ diagpσˆz { sin θi,1, ..., σˆz { sin θi,ni q (3.10) where “correct model specification” is assumed, that is, the estimation model parameters are equal to the data ˆ ˆ ˆ AR generation parameters (Ri “ Ri, Qi “ Qi and Hi “ Hi). In the constrained KF casex î “ Mpxî q converts AR the estimated along-road position into the 2-d ground-plane position for error computation, wherex î is the 1st component of the state estimate xî

In the constraint-free case the model propagator is matrix given by

1 0 δti 0 0 0 0 1 0 δti 0 0 ¨0 0 1 0 0 0˛ Fˆ “ i ˚0 0 0 1 0 0‹ ˚ ‹ ˚0 0 0 0 1 0‹ ˚ ‹ ˚0 0 0 0 0 1‹ ˚ ‹ and process˝ noise covariance is ‚

0 0 0 0 0 0 0 0 0 0 0 0 ¨0 0σ ˆ2 0 0 0 ˛ Qˆ “ v i ˚0 0 0σ ˆ2 0 0 ‹ ˚ v ‹ ˚0 0 0 0σ ˆ2 0 ‹ ˚ h ‹ ˚0 0 0 0 0σ ˆ2 ‹ ˚ ∆t‹ and the measurement˝ model is equation‚ 4.8 but with dimensionally augmented measurement matrix

0 0 0 0 0 0 pxi ´ Xi,1q{ρi,1 pyi ´ Yi,1q{ρi,1 0 0 phi ´ Zi,1q{ρi,1 1 ˆ . . . . Hi “ ¨ . . . . ˛ (3.11) 0 0 0 0 0 0 px ´ Xi,n q{ρ py ´ Yi,n q{ρ 0 0 ph ´ Zi,n q{ρ 1 ˚ i i i,ni i i i,ni i i i,ni ‹ ˝ ‚

49 representing the Jacobian of equation 3.6 with respect to the augmented state px, y, vx, vy, h, ∆tq. The modeled measurement covariance in the constraint-free estimation case is

Rˆ diag σ2 sin θ0 , ..., σ2 sin θ0 i “ p z { i,1 z { i,ni q and for error computation, the estimator pxˆi, yˆiq as a subset of xi is already in NEU co-ordinates and does not require transformation for error computation.

3.3 Evaluation of the BRMSE closed-form solution

A closed form solution to equation 3.2 may be computed having speciﬁed the estimation and data generation models in section 3.2. Solving for estimation errors in the case of a straight road section:

p1q p1q 2 MSECI “ pxˆi ´ xi q fpz1, ..., zi|x1, ..., xiq πpx1, ..., xq dX dZ (3.12) żX,Z

MSECF

p1q p1q 2 p2q 2 “ pxî ´ xi q ` pxî q fpz1, ..., zi|x1, ..., xiq πpx1, ..., xq dX dZ (3.13) żX,Z ˆ ˙ pjq th , where xi is the j component of the state vector, and CI/CF refer to which filter is being used for position p2q estimation (constraint-integrated and constraint-free respectively). The xî term in the constraint-free error expression corresponds to perpendicular-to-road displacement (in the straight-section case) which does not appear in the constrained calculation. It can be shown by combining prediction/update steps (equations 4.8 and 3.4), and fixing the linearisation point xL to be time-invariant, that the state-estimate recursion for a Kalman Filter is given by

ˆ ˆ ˆ xî “ Fi xî´1 ` Ki pzi ´ HiFi xî´1q (3.14) and by reverse-iterating the recursion in equation 3.4 to initial time, the first two components of the state estimate at time ti (for a given z1, ..., zi) can be expressed explicitly:

i nj p1q p1q p1,mq xˆi “ C0 ` Ci,j ρj,m (3.15) j“1 m“1 ÿ ÿ i nj p2q p2q p2,mq xˆi “ C0 ` Ci,j ρj,m (3.16) j“1 m“1 ÿ ÿ x B where C0 “ C0 ` C0 , and

i x C0 “ Aj x0 (3.17) j“1 ˆ ź ˙ i i B C0 “ Am Bj (3.18) j“1 m“j`1 ÿ ˆ ź ˙

50 where Ai “ Fˆ ipI ´ KiHˆ iq (3.19)

L Bj “ KjpHi xi ´ z0q (3.20) and

ˆ ˆ T ˆ ˆ T ˆ ˆ ˆ T ˆ ˆ T ˆ ´1 Kj “ pFj Pj´1 Fj ` Qjq H pHj pFj Pj´1 Fj ` QjqHj ` Rjq (3.21) is the optimal Kalman gain of zj at time tj, and Pi is the state estimate covariance matrix [25, 27]. In the case of a linear Gaussian ﬁlter, Pj satisﬁes the recursive formula:

ˆ ˆ ˆ T ˆ Pj “ pI ´ Kj HjqpFj Pj´1 Fj ` Qjq (3.22)

pm,nq th thus easily obtaining the covariance from an initial estimate P0. Ci,j corresponds to the pm, nq entry of the matrix i Ci,j “ Am Kj (3.23) m“j`1 ˆ ź ˙ Substituting these expressions into the error terms within equation 3.2, the BRMSE becomes a sum of moments following binomial expansion of the resulting expression. To complete the remainder of the calculation eﬃciently and in a closed form, two assumptions are made regarding the satellite measurement fusion:

1. Satellite positions are assumed to be static, i.e. X1,j “ ... “ Xk,j ” Xj for all j

L L L 2. The linearisation point xi is assumed to be static, that is xi ” x “ p0, 0, h0q Substituting equations 3.15 and 3.16 into 3.12 and 3.13,

MSECI “

i ni 2 p1q p1,mq p1q C0 ` Ci,j ρi,m ´ xi fpz1, ..., zi|x1, ..., xiq πpx1, ..., xq dX dZ X,Z j“1 m“1 ż ˆ ÿ ÿ ˙ i nj i np p1q 2 p1,mq p1,qq E E p1q 2 “ pC0 q ` Ci,j Ci,p pρj,m ρp,qq ` rpxi q s j“1 m“1 p“1 q“1 ÿ ÿ ÿ ÿ ˆ ˙ i nj i nj p1q p1,mq E p1,mq E p1q p1q E p1q ` 2 C0 Ci,j pρj,mq ´ 2 Ci,j pρj,m xi q ´ 2 C0 pxi q (3.24) j“1 m“1 j“1 m“1 ÿ ÿ ÿ ÿ ˆ ˙ , where the moments are with respect to the joint X ´ Z state-measurement distribution, which by construction is Gaussian. Because of this, equation 3.2 can be re-expressed as a sum of covariances and ﬁrst moments:

MSECI “

i nj i np p1q 2 p1,mq p1,qq E E pC0 q ` Ci,j Ci,p Covpρj,m, ρp,qq ` pρj,mq pρp,qq j“1 m“1 p“1 q“1 ÿ ÿ ÿ ÿ ˆ ˙ i nj p1q E p1q 2 p1q p1,mq E ` Varpxi q ` pxi q ` 2 C0 Ci,j pρj,mq j“1 m“1 ÿ ÿ

51 i nj p1,mq p1q E E p1q p1q E p1q ´ 2 Ci,j Covpρj,m, xi q ` pρj,mq pxi q ´ 2 C0 pxi q (3.25) j“1 m“1 ÿ ÿ ˆ ˙ The MSE in the constraint-free estimation case can be written as the sum of an along-road (AR) and perpendicular- to-road error (PR) term: MSECF “ MSEAR ` MSEPR (3.26) , where

i nj p2q 2 p2q p2,mq E MSEPR “ pC0 q ` 2 C0 Ci,j pρj,mq j“1 m“1 ÿ ÿ i nj i np p2,mq p2,qq E E ` Ci,j Ci,p Covpρj,m, ρp,qq ` pρj,mq pρp,qq (3.27) j“1 m“1 p“1 q“1 ÿ ÿ ÿ ÿ ˆ ˙

And MSEAR Is the same expression as in equation 3.24, albeit with the constraint-free model variants of the parameters (as deﬁned in section 3.2.2 being used to calculate the C‹ via equation 3.23. The moments can be explicitly calculated in terms of initial values, by backward-iterating equation 3.14 and substituting the resulting expression into equation 4.8. The result is to express xi and zi as linear combinations of the initialised state x0, the pre-deﬁned linearisation point xL and stochastic noise terms:

i i´1 i x xii “ Fi x0 ` Fm wj (3.28) j“1 j“1 m“j`1 ˆ ź ˙ ÿ ˆ ź ˙ i i´1 i x z L zi “ Hi Fi x0 ` Fm wj ` wi ` z0 ´ Hi x (3.29) j“1 j“1 m“j`1 ˆˆ ź ˙ ÿ ˆ ź ˙ ˙ Giving

Covpzm, znq m n T mintm,nu´1 m n T “ Hm Fj Π0 Hn Fj ` Hm Fp Qj Hn Fq j“1 j“1 j“1 p“j`1 q“j`1 ˆ ź ˙ ˆ ź ˙ ÿ ˆ ź ˙ ˆ ź ˙ m n T 1 T 1 ` mąn Hm Fj Qn Hn ` măn Hm Qm Hn Fj j“n`1 j“m`1 ˆ ź ˙ ˆ ź ˙ 1 T ` m“n Hm Qm Hn ` Rm (3.30) ˆ ˙ and

Covpzm, xnq m n T mintm,nu´1 m n T “ Hm Fj Π0 Fj ` Hm Fp Qj Fq j“1 j“1 j“1 p“j`1 q“j`1 ˆ ź ˙ ˆ ź ˙ ÿ ˆ ź ˙ ˆ ź ˙

52 m n T ` 1mąn Hm Fj Qn ` 1măn Hm Qm Fj ` 1m“nHm Qm (3.31) j“n`1 j“m`1 ˆ ź ˙ ˆ ź ˙

where Π0 is the initial estimation covariance for x0 and

i Epxiq “ Fi x0 (3.32) j“1 ˆ ź ˙ i L Epziq “ Hi Fi x0 ` z0 ´ Hi x (3.33) j“1 ˆ ź ˙ The range-wise covariances can then be expressed in terms of elements of the above covariance matrices:

pp,qq Covpρm,p, ρn,qq “ Covpzm, znq (3.34) and p1q pm,1q Covpρj,m, xi q “ Covpzj, xiq (3.35)

3.4 Numerical evaluation of closed form BRMSE solution to generate positioning error impact dataset

The explicit solutions for the BRMSE (equations 3.25, 3.26 and 3.27) is a general solution for the average positioning error across a range of positioning environments. The environmental impacts on positioning can be determined by substituting appropriate parameters to simulate the various environments, as discussed in section 3.2.2, generating average positioning error time series in those environments (figures 3.4 to 3.7 contain some selected examples). Environmental impact metrics such as peak error, average error, recovery time from peak error etc were computed from the time series and are presented in figures 3.8 to 3.23, with a table summary in figure 3.3. The solution computed in section 3.3 may be evaluated to simulate a range of environmental factors including:

1. Whether road constraints were integrated into positioning. This is toggled by selecting the appropriate expression (3.25 or 3.26) and appropriate estimation parmeters corresponding to road-constraint integrated or un-integrated positioning (see section 3.2.2). 2. The road network geometry (either “straight section” or “right turn” at a corner). The road is modeled by a perpendicular intersection of semi-infinite line segments intersecting at p0, 0, 0q NEU (see figures 3.1 and 3.2). The right-turn was fixed to occur at t0 “ 0. In the road unassisted estimation case, this was modeled y ´1 y ´1 by assigning v0 “ 10ms , compared with v0 “ 0ms in the straight-section case. The road-constrained KF was parametrically unaffected by the right turn due to the constraints disallowing off-road positions or headings.

3. Satellite visibility level (SatVis) at each time point ti. Being able to toggle simulated satellite visibility can, for example, simulate periods of GNSS outage or intermittently low SatVis to simulate an urban canyon environment, or stable high SatVis to simulate an open obstacle-free area. The SatVis level simulation is achieved by altering the pseudorange-count dimension of the Hi, Hˆ i, Ri and Rˆ i input matrices as desired, and averaging the position estimates over all possible measurements with the selected satellite arrangement. 4. Vehicle motion environment (either cruising over the full journey duration, or including a period of stoppage involving either gradual or rapid deceleration/acceleration). These scenarios are simulated by pre-specifying the times of any dynamical variation (e.g. stop at time t “ 10, then accelerate until t “ 15), and scheduling the appropriate dynamical mode index variations at the speciﬁed switch-times. This will then ensure the

53 appropriate trajectory-generation matrices Fi and Qi are input at the right times to simulate the desired vehicular motion environment. Simulating “rapid” or “gradual” linear accelerations/decelerations of the vehicle is achieved by toggling the ηa and ηd parameters respectively. Gradual/rapid linear accelerations corresponded to ηa “ 0.1 and ηa “ 0.5 respectively, and gradual/rapid linear decelerations corresponded to ηd “ 0.9 and ηa “ 0.5 respectively.

These independent environmental factors can be combined in any permutation to simulate a broad range of positioning environments for performance impact evaluation, and the results are summarised in figure 3.3. The set of environments ultimately chosen for impact evaluation were deemed a representative set of the most common vehicular environments which encompass the vast majority of vehicular travel through urban areas. Some parameter inputs were fixed over the whole performance evaluation. They include (with all units in SI unless specified):

1. Sampling periods: δti “ ti ´ ti´1 “ 1s for all i with t0 “ 0

´1 2. Cruise mode velocity process noise: σv “ 3ms

3. Altitude process noise: σh “ 3m

4. Receiver clock bias drift parameter: σ∆t “ 5m

5. Pseudorange signal noise: σz “ 5m 6. Satellite parameters: 10 satellites, for the sake of computational simplicity uniformly spaced in the azimuth over 2π, at π{4 rad elevation and equidistant from receiver at 20,000km, and ﬁxed in ECEF co-ordinates (see section 2.2.1, ”Extended Kalman Filter” for deﬁnition). This is non-physical but assumed in order for the solution to be more readily computible, without impacting viability of results.

7. Linearisation point for the measurement generating model and KF with road constraints: xL “ p0, 10, 100, 0q 8. Linearisation point for the KF without road constraints: xL “ p0, 0, 10, 0, 100, 0q

9. Initial vector for the trajectory generation model and KF with road constraints: x0 “ p0, 10, 100, 0q

In the figures below, all references to “error” should be interpreted as “root mean-squared error” calculated from equations 3.2 and 3.1 unless explicitly clarified. References to “satellite outages” should be interpreted as referring to SatVisă4 scenarios, and the SatVis level will be specified in the figure/caption.

54 Figure 3.3: Table of computed positioning performance evaluation results over a range of environments, based on statistical models. This table summarises data in ﬁgures 3.4 to 3.18. All quantities are in metres.

Figure 3.4: BRMSE theoretical average error time series, output of equation 3.2

55 Figure 3.5: BRMSE theoretical average error time series, output of equation 3.2

Figure 3.6: BRMSE theoretical average error time series, output of equation 3.2

56 Figure 3.7: BRMSE theoretical average error time series, output of equation 3.2

Figure 3.8: Steady state BRMSE values - straight segment vehicle cruise. Values are time series averages, computed from equation 3.2.

57 Figure 3.9: Steady state BRMSE reduction due to road map data integration - straight segment vehicle cruise. Values are percent reductions in time series averages, computed from equation 3.2

Figure 3.10: BRMSE in varying SatVis levels outage scenarios - straight vehicle cruise. Values are time series averages, computed from equation 3.2

58 Figure 3.11: Low-cost positioning performance improvement due to road map integration during various levels of SatVis and cruise state. Values are percent reductions in BRMSE time series averages, computed from equation 3.2

Figure 3.12: BRMSE values for varying SatVis levels during vehicle cruise state vs right turn scenario. Values are time series averages, computed from equation 3.2

59 Figure 3.13: Impacts on low-cost positioning performance due to a vehicle stoppage during low SatVis. Values are obtained from BRMSE time series, computed from equation 3.2

Figure 3.14: Low-cost positioning performance improvement due to road constraint integration, measured as percentage BRMSE reduction. Values are ratios of graph values in ﬁgure 3.13

60 Figure 3.15: Impacts on low-cost positioning performance of a vehicle stoppage during low SatVis. Values obtained from BRMSE time series computed from equation 3.2.

Figure 3.16: Low-cost positioning performance improvement due to integrating road constraints, measured as percentage BRMSE reduction. Values are ratios of compared chart values in ﬁgure 3.15.

61 Figure 3.17: Impacts on low-cost positioning performance due to linear vehicle acceleration in low SatVis measured by BRMSE. Rapid/gradual acceleration corresponds to 3/10 seconds from stopped to cruise speed. Values are obtained from BRMSE time series computed from equation 3.2.

Figure 3.18: Impacts on low-cost positioning performance due to low SatVis measured by BRMSE. Values are obtained from BRMSE time series computed from equation 3.2.

62 Figure 3.19: Low-cost positioning improvement due to road constraint integration measured by percentage BRMSE reduction. Values obtained by taking ratios from values in ﬁgure 3.18 (grey:blue and grey:orange).

Figure 3.20: Impacts on low-cost positioning performance impacts due to very low SatVis measured by BRMSE growth rates. Values are obtained from BRMSE time series computed from equation 3.2.

63 Figure 3.21: Impacts on low-cost positioning performance due to various environments measured by recovery time from peak error. Recovery time deﬁned as the time taken for BRMSE to fall from to within 10% of steady state from peak error after end of GNSS outage or return to cruise state. Values are obtained from BRMSE time series computed from equation 3.2.

Figure 3.22: Impacts on low-cost positioning performance impacts due various environments measured by recovery time from peak error. Recovery time deﬁned as the time taken for BRMSE to fall from to within 10% of steady state from peak error after increase in SatVis. Values are obtained from BRMSE time series computed from equation 3.2.

64 Figure 3.23: Low-cost positioning performance impacts of various environments measured by recovery time from peak error. Recovery time deﬁned as the time taken for BRMSE to fall from to within 10% of steady state from peak error after increase in SatVis. Values are obtained from BRMSE time series computed from equation 3.2.

3.5 Discussion of numerical results and comparison with surveyed performance validations

The key results from the BRMSE evaluation tended to broadly reflect what has been empirically observed in the low-cost positioning literature in terms of positioning error trends for Kalman Filters. This was the case for both the KF which integrated road constraints and the constraint-free case, where the differences in performance could be clearly compared over a range of environments. In the control case of a “positioning friendly” environment which measured positioning error over a steady, straight-road cruise motion with SatVis=10, the average positioning error was steady at 4m for the KF which integrated road constraints, 5m for the KF which did not (see figure 3.8). This is a reference point relative to which the impacts of environment may be measured. A subset of the environments tested only differed by one “factor” from this control “friendly environment”. That is, only one of the characteristics of 1) high SatVis, 2) straight road or 3) steady cruise motion was altered from the control case. Such cases are referred to as “single-factor environments”, and exist to directly measure the impacts of particular hostile factors in isolation; these are noted in figure 3.3. Any other non-control environments are referred to as “multi-factor environments”; these represent more typical real-life scenarios where multiple hostile factors tend to be present simultaneously.

65 3.5.1 Performance impacts of single-factor environments on low-cost positioning Performance impacts of Urban Canyons In simple urban canyons (control environment minus high SatVis), the disruption of service was signiﬁcant when road constraints were not integrated into positioning, increasing average errors from 7m at SatVis=5 to 30m at SatVis=2 (23m abs or 328% rel increase), and to 100m at SatVis=1 (93m abs or 1300% rel increase). This was signiﬁcant compared with the reduced impacts on road-constrained positioning methods, where the error increase was from 5m at SatVis=5 to 12m at SatVis=2 (7m abs or 140% rel increase) and 40m at SatVis=1 (35m abs or 700% rel increase). All urban canyons simulated were of moderate duration (20-40 seconds of SatVis reduction to the stated levels).

Comparing the KF with/without integration of road constraints, the above error measurements represent between 25% and 60% error reduction directly due to road constraint integration into low-cost positioning in urban canyons. This is broadly consistent with literature findings (see chapter 2). This error reduction was most pro- nounced during partial GNSS signal blockages (SatVis=1 and SatVis=2) where the error reduction due to constraint integration was closer to 60% between the compared filters. These SatVis scenarios tested represent very frequent occurrences in highly built up urban centres such as Hong Kong where over 60% of roads are SatVis=1 or SatVis=2 (see figure 2.1 in chapter 2). In addition to higher average errors, it was observed that the average positioning error increased steadily with time ( 0.8 m/s) in the SatVis=2 simple urban canyon when road constraints were not integrated into positioning (see figures 3.5, 3.20). This error growth was not present in identical conditions if the KF integrated road constraints; this factor stabilised the error and it did not grow with time. The SatVis=1 and SatVis=0 scenarios resulted in error growth for both road-constrained and constraint-free KF positioning methods, however the error growth was slower in the case of the KF which integrated road constraints (see figure 3.20).

Performance impacts of unsteady/accelerating vehicle motion The BRMSE data suggested that the performance impacts of manoeuvres on low-cost positioning depended largely on the magnitude and suddenness of manoeuvre execution, and whether road constraints were integrated by the filter. Most significantly, turns executed at moderate speeds (90 degrees over 3 seconds at 35kph average speed) caused short-term error increases of 10m or 167% when road constraints were not integrated (from 6m stable average error to peak error of 16m over a service-disruptive error “spike” of average-duration 7 seconds, see figure 3.18). The positioning performance impact was reduced when road constraints were integrated resulting in a smaller 1m average-error increase from 5m to 6m, or 20% with similar error-recovery delay (see figure 3.17). Gradual manoeuvres, such as decelerations or accelerations to and from vehicle stoppages, caused less service disruption due to error increase. Gradual manoeuvres resulted in error peaks of 9.4m and 7.2m average-error for road- constrained and constraint-free positioning methods, respectively, in moderate-good GNSS conditions. It was also found that, in good GNSS conditions, positioning accuracy was slightly improved when the vehicle was stationary, with (approximately) 20% error reduction from the steady-state cruise positioning errors (see figure 3.6). These error figures and trends were broadly consistent with empirical findings from wider literature surveys, however provided a greater level of quantitative clarity and statistical significance than was deducible only from error measurements in literature surveys.

Performance impacts of common Multi-factor environments Common but more complex urban environments, such as urban canyons (low SatVis environments) combined with linear variation in vehicle motion states (linear accelerations, decelerations and stoppages) provided a greater positioning challenge that either factor in isolation. The KF integrating road constraints was able to mitigate the error increasing impacts of these environments significantly more effectively than than the constraint-free KF. Following a stoppage in a low SatVis scenario, average positioning error peaked briefly (ă5 seconds) for

66 both constrained and constraint-free KFs. In SatVis=2/SatVis=1 conditions, constraint-free positioning peaked at 26m/60m errors with a 24m steady state at SatVis=2 (see figure 3.15). In the constrained positioning case, 12m/24m errors were reached in SatVis=2/SatVis=1 conditions compared with a 8m SatVis=2 steady state in the constrained case (see figure 3.15). This corresponds to 54%/60% relative performance improvement in SatVis=2/SatVis=1 for the KF with constraints compared to the KF without constraints. In the SatVis=1 case, the road-constrained KF’s positioning error immediately stabilised following the peak at a steady-state of 20m average-error. For the constraint-free KF positioning case, error did not stabilise but continued to grow for an average 10 second period at a rapid 8m/s average rate (see figure 3.20). It was also found that recovery times during various motion change scenarios in low SatVis improved as a result of road constraint integration, when compared with the constraint-free KF positioning case (see figures 3.21, 3.22, 3.23)

3.5.2 Analysis of findings The BRMSE output generated a number of key quantitative findings that reinforced trends identified in the positioning literature whilst adding to them in terms of detail, breadth and statistical significance. The findings in many cases provided more quantitative detail than could be obtained with statistical significance from real-data performance evaluations in the literature due to unaccounted variational factors between the individual evaluations of particular low-cost positioning algorithms. In particular, the findings related to the positioning accuracy impacts of GNSS-signal loss at graded levels of SatVis cannot be replicated from evaluation data in the literature due to the difficulty in controlling for SatVis levels in real experiments. This dataset therefore represents a new quantitative understanding of urban canyon impacts which may be valuable for the development of low-cost mitigation strategies. The performance impacts of road constraints on positioning error in urban canyons are also supported by but not replicable from real-data low-cost positioning performance evaluations in the literature. In particular, the finding that PSC road constrained positioning achieves stable (i.e. non-growing) positioning error at SatVis=2 whereas constraint-free positioning is unstable at SatVis=2 is a new finding which may also guide low-cost positioning solution design. This is a statistically significant quantitative verification of the study by Cui and Ge who deduced theoretically that the optimal integration of road constraints should reduce the minimum operational satellite visibility for stable positioning from SatVis=4 to SatVis=2 though they did not specify quantitative impacts in their theory [13]. In SatVis=1, the positioning error was unstable regardless of whether constraints were integrated into positioning; this indicates a data poverty issue that needs to be addressed in future work involving data augmentation systems such as multi-sensor fusion, rather than solely the optimisation of fusion method. In addition to supporting trends in low-cost positioning performance evaluations in the literature, the BRMSE results generated original findings about the performance impacts of common urban environments which are less well-represented in the positioning literature. These include multi-factor environments, such as low SatVis scenarios combined with dynamical variation or turning motion. This included original findings about the non-linear positioning error impacts of combining urban canyons with unsteady motion and changes in trajectory which have not (as far as is known) been quantified in a statistically significant way. Important findings included the stabilising effect of vehicle stoppages during a GNSS-outage, and the durations and intensities of sudden accelerations and turns in GNSS-reduced environments. For example, it was found that the impacts of losing satellite visibility was worse for constrained positioning from SatVis=1 to SatVis=0 than from SatVis=2 to SatVis=1 (see fgure 3.10), implying a “diminishing returns on GNSS data” property not previously identified. The BRMSE results also quantified the impacts of road constraint integration on the positioning performance in each of these environments which has not been systematically evaluated in a quantitative way to date in a controlled set of experiments. They revealed that road constraint integration was most effective at reducing errors and error instability in the most hostile environments (supporting chapter 2 findings, see figures 3.20, 3.14, 3.16, 3.19). The trajectory-stabilising and correcting property of PSC constraint integration was represented in the error values which diminished peak errors significantly and eliminated error growth in some circumstances compared with constraint-free positioning. These findings are of particular use in algorithm development in chapter 4, where a PSC constraint integration approach is adopted in a new positioning algorithm.

67 3.6 Conclusions

In this chapter some important findings have been uncovered regarding the performance impacts of vehicular environment and road constraint integration on positioning. Using a newly formulated theoretical measure, the BRMSE, a broad performance-metric dataset has been generated by simulating the impacts of a range of common environments on estimation error. The design of the BRMSE and the chosen parameters of the study has ensured a robust and statistically significant evaluation of the various influences on vehicle positioning in a typical urban setting. The results of this study reinforce and are supported by trends observed in primary source algorithm validations, but could not be obtained from them alone. Findings include previously un-observed performance impacts, including positioning error stability in low GNSS visibility and motion-unsteady environments. The problems associated with aggregating primary source validations to develop a broader picture have been well documented in literature surveys (including ours in chapter 2) [42, 56, 58, 46, 57]. Issues include possible test parameter selection biases, unrepresentative sampling of environments, and inconsistencies between data sources, models and performance metrics which all preclude meaningful aggregation. Therefore it is concluded that this study has been necessary to further develop this picture. A central argument has been that optimal development of future data fusion algorithms for positioning relies on a more comprehensive and detailed understanding of the quantitative impacts of vehicular environments on positioning. Therefore this chapter acts as a necessary guide for the development of a new data fusion algorithm in the next chapter. The development of this algorithm will draw from the findings of the BRMSE analysis, particularly on the significant error stability, accuracy and continuity improvements due to PSC constraint integration in positioning hostile environments.

68 Chapter 4

The Gaussian Cluster Approximate Filter: A new low-cost solution for statistically robust urban vehicle positioning

This chapter presents a new design for a low-cost GNSS-aided positioning method which integrates road constraints, referred to as the Gaussian Cluster Approximate Filter (GCAF). The method is a Bayesian estimation algorithm which iteratively generates position estimate outputx ˆi from GNSS measurements zi at time ti and digital road map data inputs, though the method is designed to be extended to integrating other types of data such as inertial sensed data. The objective is to achieve maximally accurate (low-error) and continuous low-cost positioning across a range of positioning hostile environments in real-time with only GNSS and road maps.

Chapter 2 identified that existing low cost positioning methods do not optimally solve the non-linear problem of positioning with road constraints leading to estimation biases and higher errors, particularly at low satellite visibilities. A key challenge for positioning is that integrating road constraints presents computational difficulties, such that perfect accuracy and real-time output speed cannot be achieved and must effectively be “traded off” against each other (see section 4.1 for further discussion). The algorithm presents a novel mathematical approach to solving this problem in a sufficiently fast but statistically robust and accurate recursive-Bayesian method using Gaussian cluster approximations which is tested and validated in chapter 5. It is argued that no existing method achieves a similar level of statistical precision in representing positional probabilities while generating real-time point estimates, which the results in chapter 5 verify. Existing methods tend to simplify the complex positional probability distribution too much or do not model its evolution at all, leading to estimation biases and thus errors.

The design is motivated by the findings in chapters 2 and 3 highlighting the significant positioning error reduction levels, particularly in hostile environments, of using a PSC technique to integrate road constraints (see chapter 2, section 2.2.2 for definition), and using methods that accurately model the complex vehicle position probabilities in a road network. Gaussian cluster methods in the literature tend to oversimplify the positional probability model by either reducing the size of the cluster too significantly for computational convenience and/or not modelling probability flows across road segment boundaries, both critical to maintaining a viable representation of positional probability

69 4.1 GCAF: Algorithm methodology and quantitative models 4.1.1 Introduction The GCAF algorithm uses a recursive-Bayesian method to generate position estimates from a time-evolving probability distribution Ppxi|Ziq over the vehicle state xi based on present and past GNSS signals Zi “ tz1, z2, ..., ziu, computed using equation 2.1 at every time ti. The state is a vector of vehicle-related variables including the position and other auxiliary variables which relate to positioning (e.g. speed) and are simultaneously estimated (see section 4.1.2 for specific details). The algorithm is a series of cyclical computational steps iterated at each of these times and is described fully in section 4.2 and visualised in figure 4.8. A PSC approach to road constraint integration is adopted which implies that the state probabilities Ppxi|Ziq are always constrained to the road network, due to the demonstrated performance across a range of positioning environments of PSC methods (see chapters 2 section 2.2.2 for definition and the performance evaluation in chapter 3). A key algorithm design feature is to approximate the true state probability solution Ppxi|Ziq to equation 2.1 as a Gaussian cluster (time-evolving group of Gaussian particles/components) in a statistically accurate and robust way, meaning that the design should focus on closely approximating Ppxi|Ziq given the road constraints. The aim is to compute a more accurate solution to equation 2.1 than is achieved by existing low-cost positioning solutions in the literature [42, 23]. Apart from improving the speed of positioning output, the approximation is in fact mathematically necessary because equation 2.1, even under modest and relaxed assumptions about stochastic and road map models cannot be exactly solved in real-time (see Appendix A for proof). This is due to exponentially growing computational complexity resulting in ever increasing delays for estimate output. Exact solutions are therefore not viable for a low-cost positioning solution regardless of the processing power available. However, the literature confirms that approximations which are closer to the true solution have better positioning performance, particularly in positioning-hostile environments, than those which have simpler approximations. While it is generally acknowledged in the literature that non-linear PSC road-map-assisted positioning solutions are slower than simpler PSU map matching methods, since their better approximations of true state probabilities are more mathematically complex, Appendix A of the thesis provides mathematical proof that the exact solution is in fact not feasibly achieved in real time. Such a proof did not yet exist based on this literature review, and is important for the purpose of understanding the practical limitations of positioning with road maps which further guides the GCAF design.

4.1.2 GCAF: Estimation framework, co-ordinate systems and modeling road constraints Key components of the GCAF Estimation Framework The GCAF solution may be considered as containing 6 structural elements. They include

1. A road-constrained state space applies to the positional uncertainty initialisation Pˆpx0q and subsequent Pˆpxi|Ziq. This implies that all oﬀ-road positions have zero probability at all times. This satisﬁes the hard- constraint principle, and this element is further detailed in section 4.1.4.

2. A state uncertainty initial estimate (“initialisation”) Pˆpx0qq is necessary to start the recursive estimation process of computing subsequent Pˆpxi|Ziq. The approach will adopt a Gaussian cluster uncertainty representation for Pˆpx0qq. 3. A state-evolution model is required to propagate the uncertainty representation through time - a linear- Gaussian stochastic model is adopted represented by the Φpxi|xi´1q factor in equation 2.1 (discussed further in section 4.1.4). This model includes methods of integrating dynamics-relevant auxiliary data, such as turn- restrictions, one-way streets and traﬃc signal data.

70 4. A means of integrating the GNSS measurements/other data with Pˆpxi|Ziq is necessary - a linearised Gaussian- noise stochastic model for GNSS measurements zi (see section 4.1.4) is adopted, represented by the Λpzi|xiq factor in equation 2.1.

5. A precision-focussed method of removing non-linearities and computationally problematic components of Ppxi|Ziq (the exact solution to equation 2.1) which maintains accuracy to the true solution but ensures the positioning cycle may continue in real time indeﬁnitely. The resulting approximate solution is denoted Pˆpxi|Ziq

6. A means of position estimation from Pˆpxi|Ziq for generating user outputx ˆi is discussed in section 4.2.

The ﬁrst 4 structural elements are introduced in sections 4.1.4, and the ﬁnal 2 in the recursive-Bayesian algorithm protocol detailed in section 4.2.

4.1.3 State space and initialisation of the ﬁlter State variables

The GCAF uses a 4-variable vehicle state vector xi “ pxi, vi, hi, ∆tiq. This includes the vehicle position xi and 3 auxiliary state variables (along-ground velocity vi, altitude hi and receiver clock bias ∆ti) which are estimated iteratively at each time step to improve positioning accuracy and reduce overall uncertainty in the position estimate (as measured by variance in the Gaussian cluster). The state space is the Cartesian product of the parameter-wise vector spaces X “ X ˆ V ˆ H ˆ T .

Road constraints and along-road position co-ordinates The GCAF integrates road constraints into the positioning process, which includes road segment geography/topology as well as some related information (e.g. traffic light locations, turn restrictions of specific streets). The road map data is sourced from the OpenStreetMap (OSM) GIS database, and the road network is modeled as a connected N N graph with M nodes SN “ tx1 , ..., xM u representing road network “shape points” and/or junctions, connected by N straight 1-d line segments representing road or lane centrelines. Perpendicular-to-road displacement (PTRD) is not considered, along the width of the road as the GCAF focusses on along-road positioning (e.g. where the vehicle is along a given street in the network, disregarding lateral position), but may be extended to estimate PTRD. The connectivity (topology) of the road network is defined by an M ˆ M adjacency matrix A which uniquely defines the network in combination with SN . Altitude data is not provided in the OpenStreetMap database, and is therefore estimated as an auxiliary state variable hi. The road constraints are modeled as hard binary constraints, meaning the vehicle is assumed to be on-road (off-road positions are strictly disallowed), and trajectories must respect the topology (connectivity) of the road network. If available in OSM, turn restrictions and one-way street data are also integrated as strict constraints on vehicle position and motion (see section 4.1.4).

71 Figure 4.1: Comparison of Google-Maps (above) and OpenStreetMap road-map (below) representations of the inner-north Melbourne road network. The latter is used as input data for the GCAF, with lane/road centrelines represented as a shape-node graph connected by linear segments.

The road network topology/geography can be used to define a more efficient local co-ordinate system for positioning. A set of on-network points XN Ď X is defined to be the constrained positional state space. Additionally, px,vq the constrained state sub-space XC is defined to be the set of pxi, viq pairs which are allowed given the road constraints (all on-road positions xi P XN , coupled with the along-ground velocities vi whose heading is tangential to the road segment, or segments, on which xi lies).

If integer-labels are given to the N segments in an arbitrary way from 1, ..., N, and orient the segments such

72 that the endpoints are arbitrarily labelled “+” and “-”, this oriented numbering may be used to deﬁne a bijective parametric map M : r0,Nq Ñ XN

´ xp`1 : txu “ x Mppq “ ´ ` ´ (4.1) #xrps ` pp ´ tpuqpxrps ´ xrpsq otherwise ´ ` where xk and xk represent the 2-d latitude/longitude positions of the “´” and “`” ends of segment k. As an ´ example, the linear segment labeled 15 is represented by parameter values r14, 15q, where p “ 14 maps to x15 end of the segment, and p “ 14.2 maps to the point 20% along segment 15 as measured by straight-line distance from ´ ` x15 in the direction of x15. Note that M is deﬁned so that the half-open parametric intervals for each segment ensure its bijectivity.

The 2-d positional state space XM is, via the mapping M, reduced to a 1-d finite parametric interval. The state may therefore be re-expressed as a dimensionally reduced 4-component vector xi “ ppi, vi, hi, ∆tiq where pi is the 1-d network position parameter defined through M and vi is a corresponding 1-d along-road speed variable defined as a local time-rate-of-change of pi.

Initialisation of the Gaussian cluster Pˆpx0q

The initialised posterior Pˆpx0q is the initial input into equation 2.1 and is necessary to mathematically start the recursive positioning filter. Statistically, it represents any prior knowledge regarding the vehicle state at time t0, before GNSS/other measurements have been obtained. The form of Pˆpx0q is specified as a Gaussian cluster (a weighted sum of Gaussian components), since this is a computationally manageable representation that does not compromise accuracy, since it can efficiently approximate even complex distributional shapes by varying the cluster population. As discussed in chapter 2 section 2.2.1 (subsection “Gaussian Cluster Methods”), this is more effective than a point-mass cluster (e.g. particle filter) at sampling the network due to the non-zero breadth of individual components and lesser risk of “degeneracy” [32, 23, 12]. The initial cluster is given by

n0 ˆ j Ppx0q “ w0 N px0; µ0,j, P0,j,I0,j,Sjq (4.2) j“1 ÿ where

• n0 is the chosen number of components in the initial cluster j th ˆ • w0 is the weight of the j component of Ppx0q

• In general N pxi; µi, Piq is the probability density function of a multivariate Gaussian distribution with mean p v h ∆t vector µi “ pµi , µi , µi , µi q and covariance matrix Pi at time ti

th • I0,j is the initial dynamical state index assigned to the j component of Pˆpx0q

th • Sj is the integer label of the segment containing the j component of Pˆpx0q

4.1.4 Stochastic models for vehicle state dynamics and GNSS measurements Vehicle state dynamics

The propagation of Pˆpx0q (and all subsequent clusters Pˆpxi|Ziq) is a core part of the GCAF process, which models the time-evolution of uncertainty in vehicle state between GNSS measurements. It is performed by applying a vehicle state-evolution model to each component of the Gaussian cluster. A linear-Gaussian stochastic model is speciﬁed

73 for this task (see equation 4.5). This is a time-discretised form of existing empirically validated continuous-time evolution models, described by partial diﬀerential equations as seen in [25]. An advantage of a linear-Gaussian propagation is that the initial Gaussian cluster retains its Gaussian-cluster form (via the Gaussian product of ˆ Φpxi`1|xiq and Ppxi|Ziq in equation 2.1) when i “ 0.

A multiple-structure state-evolution model is adopted which allows ﬂexibility to account for changes in motion state that are typical of vehicle travel in urban environments. The structure includes 3 “dynamical state indices” (DSIs labeled 1,2,3) corresponding to “cruising”, “manoeuvring” and “stopped” motion states, respectively. This structure was initially deﬁned by Blom and Bar Shalom [6] and is implemented similarly, see section 4.2 for further implementation details. This will be referred to as “multiple-model” or MM dynamics

Parameterising this multiple-model state evolution, the state “prediction” xˆi at time ti is similar to a Kalman Filter predict step but computed for each Gaussian cluster, and for each motion state, see [6, 27, 13] for examples. The overall prediction is a weighted average of these individual predictions. In particular, for a given cluster component, the predicted state is given by:

3 j xˆi “ wj xˆi (4.3) j“1 ÿ where the individual predictions for each modeled motion state of that component

j j j xˆi “ Fi xi´1 (4.4) are based the standard linear-Gaussian propagation:

j j j xi “ Fi xi´1 ` ωi (4.5) where 1 δt{li 0 0 0 1 0 0 F1 “ i ¨0 0 1 0˛ ˚0 0 0 c‹ ˚ ‹ ˝ ‚

1 δt{li 0 0 0 1 0 0 F2 “ i ¨0 0 1 0˛ ˚0 0 0 c‹ ˚ ‹ ˝ ‚

1 0 0 0 0 0 0 0 F3 “ i ¨0 0 1 0˛ ˚0 0 0 c‹ ˚ ‹ where ˝ ‚

• li is the length in metres of the segment corresponding to pi j • The noise term ωi is a 0-mean multivariate Gaussian vector with DSI-speciﬁc covariance matrix as given below • c is the speed of light

74 0 0 0 0 2 1 0 σv,1 0 0 Qi “ ¨ 2 ˛ 0 0 σh 0 ˚0 0 0 σ2 ‹ ˚ ∆t‹ ˝ ‚

0 0 0 0 2 2 0 σv,2 0 0 Qi “ ¨ 2 ˛ 0 0 σh 0 ˚0 0 0 σ2 ‹ ˚ ∆t‹ ˝ ‚

0 0 0 0 0 0 0 0 Q3 “ i ¨0 0 0 0 ˛ ˚0 0 0 σ2 ‹ ˚ ∆t‹ where the˝ superscripts denote‚ the dynamical state index (DSI) designating the motion state of the vehicle. A 2 1 constraint is σv,2 ą σv,2 since there is more stochastic uncertainty in the motion of a manoeuvring vehicle than a cruising vehicle where positions are more predictable (see chapter 5 for parameter values in testing). The DSI itself evolves in time as the vehicle behaviour and motion states change over its journey, and this is reflected in the dynamical model with a Markov DSI switching feature. Specifically, a 3ˆ3 Markov DSI switching matrix M is defined where the entry Mpi,jq is the time-invariant probability of DSI switch from state i to state j at a given sampling time. Switching is assumed to occur at every sampling time, though the dynamical state can remain invariant by “switching” to itself.

The dynamical motion model must also take into account motion across the boundaries of segments (“segment transition”), which may involve a trajectory selection process at the boundary point if it is a junction (common point of 3 or more segments). If there is no trajectory ambiguity in the boundary crossing (e.g. the crossing of j a shape point or junction, e.g. figure 4.2), the only dynamical model protocol is to transition to the Fi matrix with entries appropriate for the new segment. If the point is a junction, then the state-propagation must assign probability weights to each possible trajectory reflecting the likelihood of taking them (see figure 4.2). This in effect multiplies the number of hypothetical directions in the predicted state, to include all appropriately weighted combinations of motion state and trajectory choice:

nT 3 T j xî “ wj wk xî (4.6) k“1 j“1 ÿ ÿ T where wk are the trajectory likelihood weights over the nT trajectories possible at the junction encountered between time The multiple-model linear-Gaussian hidden-Markov state propagation model is integrated into the recursive Bayesian estimation as the probability density function Φpxi|xi´1q in equation 2.1. The state constraints on this density function ensures that positional estimates always adhere to the strict road constraints due to the 0 off-road probability assignment in Φ, reducing errors associated with off-road displacement.

75 Figure 4.2: Visualisation of how the GCAF splits the Gaussian component into multiple truncated parts following a propagation and junction crossing. The weights of the truncated “pieces” correspond to the probabilities of taking the corresponding directions, following the mathematically exact solution.

Integrating turn-restrictions and one-way-street trajectory constraint data into positioning Turn restrictions and one-way-street designations are important road constraint data that can improve the accuracy of the vehicle state evolution model Φpxi`1|xiq and low-cost positioning overall. Turn restrictions are integrated by assigning turn trajectory weights to zero, with reference to the method described in the preceding section for weighting Gaussian clusters which cross junctions. With no restriction, the component will divide into nJ components on each of the segments radiating from J, with the initial weight dividing equally by nJ and assigned to each resulting cluster component. If there is a turn restrictions, the cluster which is propagated in the forbidden direction is assigned a “0” weight, and weight multipliers are uniformly split between the allowed trajectories from the junction (see ﬁgure 4.3).

76 Figure 4.3: Visualisation of how the GCAF splits a Gaussian component following a junction crossing including a one-way street / no turn restriction (designated by arrows). No probability mass ﬂow into the forbidden direction reﬂects the modeled zero probability of such a turn, and thus zero weight attached to the corresponding piece.

Traffic signal location data integration (TSLDI) Traffic signal location data (TSLD) are another source of low-cost data which may be integrated to improve positioning. Traffic signals have a significant impact on the dynamics of vehicles through an urban road network, and therefore the knowledge of their locations may be integrated into the positioning process to improve prediction accuracy and thus overall positioning accuracy.

If TSLD is available, the traffic signal locations are matched to their corresponding network nodes. If a traffic signal location does not correspond to an existing node in the network segmentation, one is added at the traffic signal location and the network re-structured and re-labeled. A “traffic signal proximity zone” (TSPZ) is defined as a subset of the network positional space XTSPZ Ď XN where XTSPZ is the set of all locations in the road network that have a network-connected path to a traffic signal that is less than a pre-defined distance dTSPZ (see figure 4.4).

When a cluster component enters a TSPZ (xi P XTSPZ¯ X XN ), the switching matrix toggles to a TSPZ-speciﬁc

77 switching matrix MTSPZ rather than the standard matrix M (defined in section 4.1.4, sub-section “Stochastic dynamical model for hidden-Markov state propagation”). The dynamical state switching probabilities (which are the entries of MTSPZ) are selected to increase the likelihood of transitioning to a “stopped” or “manoeuvring” state to reflect the increased likelihood of a stoppage or deceleration near a traffic signal compared with vehicular environments which favour more regular and predictable cruise motion. The effect of the TSLD-integrated model is therefore, via MTSPZ, to redistribute cluster component weights to more accurately represent the stochastic vehicle propagation around a traffic signal.

Figure 4.4: Visualisation of Traffic Signal Proximity Zones (TSPZs) surrounding each traffic signal, indicating where the GCAF applies particular dynamical models for more accurate position estimation in the vicinity of traffic signals.

Measurement model for code-based GNSS pseudorange measurements GNSS measurements integrate with the modeled evolution (“prediction”) of vehicle state (described in the immediately preceding 2 sections) to provide a correction to the predictions. The eﬀect is an adjustment on the centre/spread of each Gaussian cluster component, and a re-weighting, based on a measurement model (see section 4.2.4 for mathematical details of implementation). Measurement models for both code-based pseudoranges and single-point position (SPP) type GNSS pseudoranges are introduced. The GCAF will integrate these models in the data fusion step to be capable of handling both types of data. j th Code-based GNSS pseudoranges are ρi are the raw distance estimates from the j satellite to the vehicle’s GNSS receiver at time ti. They are linked to the vehicle’s true position via the pseudorange equation:

j R S ρi ” fpxi , Xi q ` wi,j

S R 2 S R 2 S R 2 R R “ pxi,j ´ xi q ` pyi,j ´ yi q ` pzi,j ´ zi q ` c ∆ti,j ` wi,j (4.7) b

78 where

R • xi is the position of the vehicle’s GNSS receiver in ECEF co-ordinates at time ti • XS XS ,XS , ..., XS is the set of satellite positions in ECEF co-ordinates at time t i “ t i,1 i,2 i,Ki u i

S S S S th • Xi,j “ pxi,j, yi,j, zi,jq are the co-ordinates of the ECEF position of the j satellite at time ti • c is the speed of light

R • ∆ti,j is the receiver clock bias

R th • wi,j is the additive GNSS-pseudorange white noise of the j signal at time ti, modeled as a 0-mean Gaussian 2 2 σz variable with variance σi,j “ 2 sin pθi,jq

2 • σz is a minimum atmospheric noise parameter for pseudorange measurements th • θi,j is the azimuth (elevation) angle from the vehicle’s GNSS receiver to the j satellite at time ti

1 Ki Therefore, the main design of the GCAF is such that the measurements zi “ pρi , ..., ρi q acquired at every sampling time ti are integrated in their raw form into positioning (“tight coupling”). Note that the satellite visibility Ki is equal to the number of pseudoranges obtained by the receiver at time ti. A later section discusses the integration of pre-processed GNSS “measurements” such as SPP (single-point-positions), which have been computed using a prior process and are not sensed data, but are often considered measurement inputs.

A measurement model is required to integrate (fuse) GNSS data with dynamical predictions and the road constraints. To be compatible with the recursive Bayes equation 2.1 it is expressed as a probability density function (pdf), which is defined to be the likelihood function Λpzi|xiq. The likelihood function expresses the probability of observing the sensed information zi given a hypothesised hidden-state xi. The zi Ñ ppi, vi, hi, ∆tq (measurement to hidden-state) statistical link is derived from the range equation 6.3 (up to a simple linear co-ordinate transform from the ECEF proxy-state to the hidden state x¯i “ pxi, yi, zi, ∆tiq Ñ xi “ ppi, vi, hi, ∆tiq, see section 4.2 for details). However, the likelihood is non-Gaussian in x¯i if the pdf is derived from equation 6.3 in its non-linear form. This translates to the solution Ppxi|Ziq of equation 2.1 being non-Gaussian, and therefore impractically difficult to compute. Therefore the non-linear model (equation 6.3) is replaced by its first-order Taylor expansion with respect S R to xi by evaluating derivatives at a chosen linearisation point x0 :

zi “ z0,i ` H¯i px¯i ´ x¯i,0q ` wi (4.8) where:

1 Ki • z0,i “ pρ0,i, ..., ρ0,i q is the “reference measurement” at time ti

j R S • ρ0,i “ fpx0 , xi,jq are the “reference ranges”

∇T fpxR, xS q R i,j R S pxi,0,xi,1q . ˇ • H¯ “ ¨ . ˇ ˛ is the measurement matrix in ECEF co-ordinates i . ˇ ˚ ‹ ∇T fpxR, xS q ˚ R i,j R S ‹ ˚ pxi,0,xi,K q‹ ˚ ˇ i ‹ ˝ ˇ ‚ B ˇ • ∇R “ is the ECEF receiver-state gradient vector BxS ˆ i ˙

79 • wi is a zero-mean Gaussian noise term with covariance σ2 σ2 Σ “ diag ρ , ... , ρ z sin θ sin θ ˆ i,1 i,Ki ˙ th • θi,j is the zenith (elevation) angle from the vehicle’s receiver to the j satellite.

Measurement model for SPP-type GNSS measurements

SPP x y SPP measurements are of latitude/longitude point-position estimate type zi “ pzi , zi q (typically pre-computed using a least-squares solution to equation 6.3 or a similar filtering method), rather than sets of pseudorange distance measurements. This extension is introduced because pre-calculated or pre-filtered GNSS point-position estimate “measurements” are common inputs into data fusion algorithms for positioning, and therefore should be considered. It is also to demonstrate the flexibility of the GCAF as a methodological approach to a variety of input data sources without any structural changes necessary to its protocol.

The GCAF must be provided with an appropriate measurement model when SPP measurements are used. A standard linear-Gaussian model is adopted:

SPP ¯ zi “ H x¯i ` wi (4.9) where

• x¯i “ pxi, yi, zi, ∆tiq where pxi, yiq is latitude/longitude, zi is altitude and ∆ti is receiver clock bias 1 0 0 0 • H¯ “ i 0 1 0 0 ˆ ˙ σ2 0 • w is a zero-mean Gaussian noise term with covariance Q “ z i 0 σ2 ˆ z ˙

This provides the necessary statistical link between SPP measurements to the proxy-state x¯i which through a simple linear co-ordinate transform links to the road-constrained state xi “ ppi, vi, hi, ∆tiq (see section 4.2 for details).

4.2 Bringing the elements together: Mathematical solution to the Bayes equation for the GCAF position estimator

The real-time recursive approximate solution to the positioning equation 2.1 produces a probability distribution from which the positioning output is generated. Its solution with respect to the stochastic models for the GCAF as described in the preceding sections is computed below. The computation process of this solution can be described as a sequence of looping steps repeated at each time ti where position estimate output is required, following from the initialisation (see ﬁgure 4.8). These steps integrate all 6 of the broad structural elements described in section 4.1.2, and are described mathematically in this section.

4.2.1 Gaussian cluster evolution

The GCAF is initialised as a Gaussian cluster (see section 4.1.3). At the ﬁrst iteration (time t1), the linearised multiple-model dynamic process (see section 4.1.4) propagates the initial cluster by acting on each cluster compo- ˆ ˆ nent. The forward-propagated posterior distribution is denoted Ppxi´1|Zi´1q Ñ Ppxi|Zi´1q and is computed as below:

80 ni´1 ˆ j Ppxi´1|Zi´1q “ wi´1 N pxi´1; µi´1,j, Pi´1,j,Ii´1,j,Si´1,jq (4.10) j“1 ÿ ki ˆ j ˆ Ppxi|Zi´1q “ wi N pxi; µˆi,j, Pi,j,Ii,j,Si,jq (4.11) j“1 ÿ where

• ni is the number of components in the posterior cluster following the completion of the GCAF iteration at time ti

• ki is the number of components in the propagated posterior cluster at time ti. As explained below, ki ě ni

j th • wi is the weight of the j component at time ti th • Ii,j is the dynamical state index of the j component at time ti

th • Si,j is the segment containing the spatial-mean of the j component at time ti

The propagated component parameters µˆi,j and Pˆ i,j are computed using the linear dynamical model:

Ii,j µˆi,j “ Fi µi´1,j (4.12)

ˆ Ii,j ˆ Ii,j T Ii,j Pi,j “ Fi Pi,j pFi q ` Qi (4.13)

4.2.2 Handling segment boundary crossing during propagation The results in equations 4.12 and 4.13 are correct without adjustment if the centre of the component does not cross any boundaries in the forward-propagations (i.e. tpi´1,ju “ tpˆi,juq.

If the component has crossed a segment boundary ptpi´1,ju ‰ tpî,juq, then the forward-propagated parameters are adjusted via an additional segment-transition protocol step. Firstly, it must be determined if the boundary point is a shape point or a junction. If a shape point, then the trajectory is known andp î,j is the network parameter value corresponding to the appropriate position along the neighbouring segment to Si´1,j, as determined by the physical “over-shoot” past the boundary point when evaluating equation 4.5. The auxiliary state values vi,j, hi,j and ∆ti,j are unaffected by the boundary crossing and are un-modified after evaluating equation 4.5.

Following segment transition, the propagated covariance Pˆ i,j must be appropriately re-scaled because the physical scale of the p-variance has different physical meaning on segments of different length (since it is a dimensionless ratio of the segment length). This is a two-stage process where the covariance is first dimensionally propagated to compute the covariance of the state-propagated Gaussian component in dimensional units in place of the p-covariates (by a factor-scaling in the length li´1,j of the original segment):

ˆ x ˆ T T Pi,j “ Fi´1,j Ti´1,j Pi´1,j Ti´1,j Fi´1,j ` Qi´1,j (4.14) where

• Fi,j is the state propagation matrix speciﬁc to the segment Si,j and dynamical state index Ii,j

81 li,j 0 0 0 0 1 0 0 • T “ is a position-dimensionalising matrix i,j ¨ 0 0 1 0˛ ˚ 0 0 0 1‹ ˚ ‹ ˝ ‚ • li,j is the length of the segment with integer label Si,j

• Qi,j is the state propagation process-noise covariance corresponding to the dynamical state index Ii,j The dimensionally propagated covariance is then re-scaled to correctly reﬂect the covariates of the dimensionless network parameter p on the new segment Si,j:

ˆ ˆ x T Pi,j “ Ui,j Pi,j Ui,j (4.15) ´1 where Ui,j “ Ti,j .

If the boundary point is a junction with n choices of subsequent trajectory, then the Gaussian will split into n components, weighted by the likelihoods of choosing the corresponding trajectories (multi-hypothesis prediction, see section 4.1.4 and ﬁgure 4.2), e.g.

n pmq ˆ pmq pmq N pxi´1; µi´1,j, Pi´1,j,Ii´1,j,Si´1,jq Ñ wm N pxi; µi,j , Pi,j ,Ii´1,j,Si,j q (4.16) m“1 where ÿ

• wm is the trajectory likelihood weight • n is the number of segments sharing the crossed boundary point

pmq th • Si,j is the integer label of the m segment sharing the crossed boundary point

pmq • µi,j is the state prediction given by evaluating equation 4.5 for vector components vi, hi, and ∆ti,j (inde- pmq pmq pendent of m), and using the “over-shoot” principle to determine pi,j on segments Si,j ˆ pmq pmq • Pi,j is the propagated covariance scaled using the lengths of Si,j and Si´1,j, computed in the same way as the shape-point (trajectory unambiguous) case above, then repeated for all trajectories m. The propagated (“prediction”) prior may be expressed as the sum of the weighted components as computed using the above protocol:

ni ˆ i ˆ Ppxi|Zi´1q “ wˆj N pxi; µˆi,j, Pi,j,Ii´1,j,Si,jq (4.17) j“1 ÿ where

• ni is the number of Gaussian components following cluster propagation. th • Ii´1,j is the dynamical state index of the j component before propagation. If the component has been generated from a junction-split, then the index is the same as that of its parent component before propagation.

i th • wˆj is the weight of the j component of the propagated prior. Its value is the same as before propagation i unless the component has crossed a junction, in which casew ˆj is the before-propagation weight multiplied by the trajectory likelihood weight. ˆ If desired, the predicted state xî and positionx î can be efficiently computed from Ppxi|Zi´1q via a (least-square error optimal) weighted mean, or other protocol.

82 4.2.3 Component sampling regions Because the Gaussian pdf is everywhere-positive, Gaussian components ˆ N pxi; µî,j, Pi,j,Ii´1,j,Si,jq spread their probability mass to unbounded extent throughout the non-linear road network space XN (non-finite boundary crossings). This presents a technical issue as only a finite amount of information about a component may be stored. The propagation step includes a “spatial confinement” process to address this problem.

ˆ Spatial confinement restricts the probability mass flow of each component N pxi; µî,j, Pi,j,Ii´1,j,Si,jq to the segment Si,j and its 2-sigma neighbourhood. That is, the “neighbourhood” of Si,j (denoted Ri,j) is all segments ˆ p1,1q which contain a point within a 2σi,j shortest-network-route distance ofµ î,j (in metre units) where σi,j “ li,j Pi,j ˆ (see figure 4.6). Ri,j is referred to as the sampling region of N pxi; µî,j, Pi,j,Ii´1,j,Si,jq. The GNSS measurementb update can then be applied to the finite sampling regions.

4.2.4 GNSS measurement integration into positioning ˆ The propagated posterior Ppxi|Zi´1q is updated by GNSS measurements by applying the measurement model Λpzi|xiq to each segment in each of the Ri,j. The GNSS-updated posterior is then expressible as an aggregate sum of segment-wise GNSS updates over all segments within each Ri,j by applying quadrature linearity to equation 2.1. Let us consider a single sampling region Ri,j belonging to ˆ ˆ N pxi; µî,j, Pi,j,Ii´1,j,Si,jq; the corresponding spatially-confined component (denoted Pjpxi|Zi´1q) may be written as a finite sum of truncated Gaussian components on the segments in Ri,j, with segment-specific location and shape parameters:

Ni,j ˆ 1 ˆ Pjpxi|Zi´1q “ piPSi,j,k N pxi; µˆi,j,k, Pi,j,k,Ii´1,j,Si,jq (4.18) k“1 ÿ where

• Ni,j is the number of segments within the set Ri,j

th • Si,j,k is the integer label of the k segment in the set Ri,j

• µî,j,k is the “virtual mean” of the truncated component on segment Si,j,k, which is equal toµ î,j, except that the p-component is replaced withx î,j,k which lies on the virtual linear extension of Si,j,k (see figure 4.5).

T • Pˆ i,j,k “ Ui,j,k Ti,j Pˆ i,j pUi,j,k Ti,jq , is the k-speciﬁc covariance with matrices U and T deﬁned in the “Seg- ment boundary crossing during propagation” subsection above, with appropriate subscript correspondence, and li,j,k is the length of Si,j,k

• pi is the p-component of the state vector xi

83 Figure 4.5: Visualisation of the “virtual mean” of the propagated Gaussian component ˆ N pxi; µˆi,j,k, Pi,j,k,Ii´1,j,Si,jq, used by the dynamical model to determine on which segment the centre of the propagated Gaussian component should be situated.

The un-normalised component-wise updates integrate the GNSS measurements with the propagated posterior:

Ni,j ¯ 1 ˆ Pjpxi|Ziq “ piPSi,j,k Λpzi|xiq N pxi; µˆi,j,k, Pi,j,k,Ii´1,j,Si,jq k“1

ÿ Ni,j 1 “ piPSi,j,k γi,j,k N pxi; µi,j,k, Pi,j,k,Ii´1,j,Si,jq (4.19) k“1 ÿ where

• Λpzi|xiq “ N pzi; z0, riq σ2 σ2 • z “ pρ1 , ρ2 , ..., ρKi qT and r “ diag ρ , ... , ρ in the pseudorange case (see section 4.1.4) 0,i 0,i 0,i 0,i i sin θ sin θ ˆ i,1 i,Ki ˙ j T • ρ0,i “ pXi,j ´ λpxiqq pX ´ λpxiqq

• λpxiq mapsa the point pMppiq, hiq in latitude-longitude-altitude (LLA) co-ordinates to their Earth-Centred- Earth-Fixed (ECEF) co-ordinate image

2 2 • z0 “ Mppiq and ri “ diagpσz , σz q in the SPP measurement case

84 • The closed-form expression for γi,j,k is calculated in Appendix B for both the pseudorange and SPP measurement cases The linear-Gaussian measurement update therefore preserves the Gaussian structure of the un-normalised posterior, with parameter adjustmentsµ ˆi,j Ñ µi,j and Pˆ i,j Ñ Pi,j on individual segments mirroring a linear-ﬁlter form:

µi,j,k “ µˆi,j,k ` Ki,j,k θi,j,k (4.20) where ˆ T T ˆ ´1 • Ki,j,k “ Pi,j,k Hi,j,kpHi,j,k Pi,j,k Hi,j,k ` rjq is the segment-wise linear gain

• ri is as deﬁned in equation 4.19

0 • θi,j,k “ zi ´ zi,j,k is the GNSS measurement residual

0 R • zi,j,k is as z0,i deﬁned in equation 4.8 in the pseudorange case, with x0 Ñ µˆi,j,k

0 • zi,j,k “ Mppˆi,j,kq in the SPP measurement case

• pˆi,j,k is the p-component ofµ ˆi,j,k

• In the pseudorange measurement case, the measurement matrix Hi,j,k “ H¯i,j,k Ci,j,k Di,j,k, where H¯i,j,k is as deﬁned in equation 4.8 ´ sin λ cos λ 0 0 0 ´ sin φ cos λ ´ sin φ sin λ cos φ 0 0 ¨ ˛ • Ci,j,k “ cos φ cos λ cos φ sin λ sin φ 0 0 is a NEUÑECEF state co-ordinate transform matrix ˚ 0 0 0 1 0‹ ˚ ‹ ˚ 0 0 0 0 1‹ ˚ ‹ (northing-easting-up˝ to Earth-centred-Earth-ﬁxed, NEU‚ measured from arbitrary origin)

• λ is shorthand for λi,j,k, the longitude of the point µˆi,j,k

• φ is shorthand for φi,j,k, the geodetic latitude of the point µˆi,j,k

dxi,j,k 0 0 0 dyi,j,k 0 0 0 ¨ ˛ • Di,j,k “ 0 0 1 0 is a xi Ñ NEU state co-ordinate transform matrix ˚ 0 1 0 0‹ ˚ ‹ ˚ 0 0 0 1‹ ˚ ‹ ˝ ‚ • dxi,j,k and dyi,j,k are respectively the East-West and North-South distance spans of the segment with integer label Si,j,k SPP Note that for brevity, in the above list zi is equivalent to zi as deﬁned in section 4.1.4, subsection “Measurement model for SPP-type GNSS measurements” when considering the case of SPP-type measurements. The measurement update to the covariance is also segment-speciﬁc:

Pi,j,k “ pI ´ Ki,j,k Hi,j,kq Pˆ i,j,k (4.21)

The un-normalised posterior is then explicitly given by aggregating the P¯jpxi|Ziq:

ni ¯ i ¯ Ppxi|Ziq “ wj Pjpxi|Ziq (4.22) j“1 ÿ 85 i where the wj are as deﬁned in equation 4.11.

Figure 4.6: Visualisation of the propagation and GNSS measurement re-weighting of a Gaussian cluster used by the GCAF in estimation, which can result in components multiplying. The component splits after being propagated across the junction and the resulting truncated pieces nearest the GNSS measurement zi are given greatest weight multipliers, resulting in two new components with signiﬁcant weight.

¯ 4.2.5 Reducing computational complexity of Ppxi|Ziq for subsequent iterations ˆ As shown in Appendix A, the linear GNSS measurement update of the forward-propagated posterior Ppxi|Zi´1q causes non-alignment of probability density across segment boundaries due to orientation sensitivity (Lemma 1). Without adjustment, the non-alignment results in non-Gaussian structural distortions to the GNSS-updated truncated components in future iterations and exponentially growing computational complexity. The exact (to-date) GNSS-updated posterior P¯pxi|Ziq is therefore infeasible and must be adjusted to generate a more computationally ‹ eﬃcient approximation P¯ pxi|Ziq.

1 A moment-matching approximation method replaces each truncated Gaussian component piPSi,j,k γi,j,k N pxi; µi,j,k, Pi,j,k,Ii´1,j,Si,jq ‹ ‹ with a Gaussian component N pxi; µi,j,k, Pi,j,k,Ii´1,j,Si,jq that has equal ﬁrst and second moments, such that

‹ E µi,j,k “ pTN i,j,kq (4.23) ‹ Pi,j,k “ Cov pTN i,j,k, TN i,j,kq (4.24)

86 1 where TN i,j,k is the random quantity whose probability density function is piPSi,j,k N pxi; µi,j,k, Pi,j,k,Ii´1,j,Si,jq up to a normalisation factor. This deﬁnes an approximate un-normalised posterior solution:

ni Ni,j ¯‹ i ‹ ‹ P pxi|Ziq “ wj αi,j,k N pxi; µi,j,k, Pi,j,k,Ii´1,j,Si,jq (4.25) j“1 k ÿ ÿ where

αi,j,k “ γi,j,k N pxi; µi,j,k, Pi,j,k,Ii´1,j,Si,jq dx (4.26) żSi,j,k combines truncation and prior/update weights. The result of this constrained moment-matching method is statistical accuracy of the approximation (eﬃcient preservation of key structural/qualitative features of P¯pxi|Ziq) whilst removing the computational issues. Since moment-matching is non-trivial for multivariate Gaussian sections which are truncated in certain covariates, it is necessary to derive the closed-form expressions for 1st and 2nd moments of a truncated multivariate Gaussian.

Let us consider a multivariate Gaussian in n variables, X “ pX1,X2, ..., Xnq with mean vector µ “ pµ1, µ2, ..., µnq “ EpXq and covariance matrix Σi,j “ E pXi ´ µiq pXj ´ µjq . If truncation is performed on the single variable X1 1 on the range X1 P ra, bs (by multiplying the pdf N pX; µ, Σq by a factor X1Pra,bs and re-normalising),the trun- “‹ ‹ ‹ ‹‰ ‹ cated multivariate Gaussian vector X “ pX1 ,X2 , ..., Xnq is obtained. The moments of the X are denoted ‹ ‹ ‹ ‹ E ‹ ‹ E ‹ ‹ ‹ ‹ µ “ pµ1, µ2, ..., µnq “ pX q and Σi,j “ pXi ´ µi q pXj ´ µj q . It can be shown, by applying the deﬁnition of statistical expectation, that “ ‰

‹ E µ1 “ pX1q “ µ1 ` σ1 ξ (4.27) ‹ E µj “ pXjq “ µj ` ρ1,j σj ξ (4.28) 2 ‹ E 2 σ1 “ ppX1 ´ µX1 q q (4.29) 2 2 “ σ1 p1 ` ν ´ ξ q 2 ‹ E 2 σj “ ppXj ´ µXj q q (4.30) σ2 1 ρ2 ν ξ2 “ j p ` X1,Xj p ´ qq ‹ Cov pX1,Xjq “ ErpX1 ´ µ1q pXj ´ µjqs (4.31) 2 “ ρ1,j σ1 σj p1 ` ν ´ ξ q ‹ Cov pj, kq “ ErpXj ´ µjq pXk ´ µkqs (4.32) 2 “ σj σkpρj,k ` ρ1,j ρ1,kpν ´ ξ qq

where

2 • σi “ Σi,i

|Σ | 1{2 • ρ “ sgn pΣ q i,j i,j i,j σ σ ˆ p q ˙ 0 : x “ 0 • sgn pxq “ x $ otherwise &|x|

φpz0q% ´ φpz1q • ξ “ Φpz1q ´ Φpz0q

87 z φpz q ´ z φpz q • ν “ a a b b Φpzbq ´ Φpzaq

a ´ µ1 xb ´ µ1 • za “ and zb “ σ1 σ1 x • Φ “ ´8 φpξq dξ

ş 1 2 • φpxq “ ? e´x {2 2 π ‹ ‹ The moment-matched parameters µi,j,k and Pi,j,k can be expressed in closed-form by applying equations 4.27- 4.32 to the state vector xi by substituting:

• X1 “ pi,j,k

• Xi and/or Xj for other state variables as appropriate

• µ “ µi,j,k

‹ ‹ • µ “ µi,j,k

• Σ “ Pi,j,k

‹ ‹ • Σ “ Pi,j,k

• Integer values Si,j,k ´ 1 and Si,j,k respectively for a and b

88 Figure 4.7: Visualisation of how the GCAF approximates a truncated Gaussian (above) with a non-truncated Gaussian component with equal 1st and 2nd moments (red). This approximation method is applied to all truncated components which result from measurement updates (see ﬁgure 4.6) in order to reduce computational burden while accurately approximating the true probability distribution solution to equation 2.1.

‹ The GNSS-updated approximate un-normalised posterior solution P¯ pxi|Ziq (see equation 4.25) may be re- expressed in a more notationally eﬃcient form by aggregating all components into a single summation, thereby dropping a subscript:

Ni ¯‹ ‹ ‹ P pxi|Ziq “ γi,j N pxi; µi,j, Pi,j,Ii´1,j,Si,jq (4.33) j“1 ÿ where

‹ ‹ • The subscripts i, j correspond to distinct segments Si,j, each of which contains a single component N pxi; µi,j, Pi,j,Ii´1,j,Si,jq within the cluster

ni • Ni “ j“1 Ni,j is the total number of Gaussian components in the approximate un-normalised cluster ř j ‹ ‹ • γi,j are the subscript-reduced weight products wi αi,j,k which multiply the components N pxi; µi,j,k, Pi,j,k,Ii´1,j,Si,jq

89 ‹ ‹ • µi,j and Pi,j are the subscript-reduced parameters of the components which are weighted by γi,j The moment-matched Gaussian components are then re-assigned motion-state indices via the multiple-model dynamics.

4.2.6 Modeling and estimating the evolution of vehicle motion state

In line with the multiple-model state-evolution method, the dynamical state indices Ii´1,j of each GNSS-updated ‹ Gaussian component t1, 2, ..., j, ..., Niu of P¯ pxi|Ziq need to be propagated according to the Markovian dynamical state time-evolution model for the beginning of the next GCAF iteration (see section 4.1.4). The transition Ii´1 Ñ Ii,j is stochastic and modeled by splitting of the moment-matched Gaussian components into sub-clusters of weighted components:

‹ ‹ pIi´1,j ,kq ‹ ‹ N pxi; µi,j, Pi,j,Ii´1,j,Si,jq Ñ M N pxi; µi,j, Pi,j, k, Si,jq (4.34) k“1 ÿ ‹ where M is the Markov dynamical state switching matrix as deﬁned in section 4.1.4. The Gaussian cluster P¯ pxi|Ziq, after dynamical-state propagation, is therefore given by

Ni 3 ~ ‹ ‹ ‹ P pxi|Ziq “ βi,j,k N pxi; µi,j, Pi,j, k, Si,jq (4.35) j“1 k“1 ÿ ÿ pIi 1,j ,kq where βi,j,k “ M ´ γi,j. By re-labelling the components by merging subscripts tj, ku into a single component- counting index j and correspondingly k “ Ii,j, βi,j,k “ βi,j, the state-propagated cluster can be re-expressed as a single sum:

3Ni ~ ‹ ‹ ‹ P pxi|Ziq “ βi,j N pxi; µi,j, Pi,j,Ii,j,Si,jq (4.36) j“1 ÿ Following the dynamical state propagation, the total cluster population is trebled to 3Ni. The unregulated iteration of this process at every sampling time would therefore result in exponential cluster size growth. To prevent the corresponding computational costs from becoming prohibitive, a component merging step is included to counter this growth and stabilise the cluster population.

4.2.7 Gaussian cluster merging with multiple model dynamics ~ ‹ C ~ ‹ The cluster P pxi|Ziq generated from state index division can be grouped into Ni sub-clusters Pj pxi|Ziq where C j “ 1, 2, ..., Ni whose Gaussian components share a common dynamical state index, segment, and direction of heading. Each sub-cluster is then combined into a single Gaussian component C C κi,j N pxi; µi,j, Pi,j,Ii,j,Si,jq using a moment-match process such that ~µi,j is the mean of cluster j at time ti (deﬁned ‹ C as an expected xi value over the normalised sub-cluster Pj pxi|Ziq), and Pi,j equal to the co-variance of the same cluster:

C 1 µi,j “ βi,k µi,k (4.37) κi,j kPC ÿi,j and C 1 ‹ T Pi,j “ βi,k Pi,j ` p~µi,j ´ µi,kqp~µi,j ´ µi,kq (4.38) κi,j kPCi,j „  ÿ where

90 ~ ‹ ~ ‹ • Ci,j is the set of component-counting indices of P pxi|Ziq which correspond to the sub-cluster Pj pxi|Ziq

• κi,j “ wi,j,k is the moment-match normalisation kPC ÿi,j This method is based on the merging (“matching”) process employed in the original interacting-multiple-model algorithm by Blom and Bar Shalom (1988), and is a statistically optimal method of distribution-merging and Gaussian approximation, in line with the GCAF formulating principles [6]. This “cluster-compression” has the eﬀect of signiﬁcantly stemming cluster population growth resulting from state-evolution component-splitting, as C required for computational feasibility. The un-normalised “compressed” cluster Pˆ pxi|Ziq is then a sum of combined components:

C Ni ˆC C C P pxi|Ziq “ κi,j N pxi; µi,j, Pi,j,Ii,j,Si,jq (4.39) j“1 ÿ 4.2.8 Gaussian cluster component elimination and re-normalisation The merging method of population control does not account for cluster population growth through junction splitting (see section 4.1.4). This means that despite the index merging described above, populations may still grow at exponential rates as the cluster spreads throughout the network. This typically results in a broad sub-population of low-weight cluster components which do not meaningfully contribute to estimation but unnecessarily increase computational burden. A 3-step normalise-trim-renormalise protocol is adopted to eliminate low-weight cluster components and stabilise the cluster population.

Firstly, the remaining cluster components following the motion-index merging step are weight-normalised to ` generate a normalised posterior Gaussian cluster Pˆ pxi|Ziq:

C Ni ˆ` κi,j C C P pxi|Ziq “ N pxi; µi,j, Pi,j,Ii,j,Si,jq (4.40) Ki j“1 ÿ where C Ni

Ki “ κi,j (4.41) j“1 ÿ is the normalisation constant for the pre-trimmed posterior. Secondly, all Gaussian components from the cluster with weights lower than a pre-set cut-oﬀ value are removed i.e. κ i,j ă w´ (4.42) Ki The weights of the remaining Gaussian components are then re-normalised to produce the trimmed posterior Gaussian cluster:

C Ni ˆ C C Ppxi|Ziq “ ηi,j N pxi; µi,j, Pi,j,Ii,j,Si,jq (4.43) j“1 ÿ where

91 κi,j 1 ´ • ηi,j “ κi,j ąKi w are the normalised cluster component weights Ei Ki

C Ni κi,j • E “ 1 ´ i K κi,j ąKi w j“1 i ÿ The choice of w´ should be within an optimal range such that it effectively manages population growth (is not too low) but does not lead to biased estimation through posterior distortion (is not too high). w´ need not be fixed; a variable cut-off may be desirable where the normalised weight distribution of the cluster is known to fluctuate (e.g. due to poor GNSS visibility or variability in road network segment density). A variable cut-off may be a function of the mean/variance of the weight distribution, reject a designated quantile, or eliminate all but the NC highest-weight components.

Following weight renormalisation, the computational treatment of the posterior distribution Pˆpxi|Ziq at time ti has been completed and it has a Gaussian cluster form. Therefore it can enter the next iteration of data fusion at time ti`1.

ˆ 4.2.9 Computing position estimates from Ppxi|Ziq

Before re-commencement of the iterative protocol, a position point-estimatex î is then computed from Pˆpxi|Ziq for output to the user. Other auxiliary outputs may be generated such as point estimates of other quantities (e.g velocity vi or altitude hi) and/or uncertainties/confidence intervals in the point estimates from the marginal distributions of Pˆpxi|Ziq. A minimum-square-error estimation criterion is used for generating point estimates from Pˆpxi|Ziq, i.e. a posterior expectation estimator. The error is defined as the straight-line distance between the estimator and true position.

The co-ordinate transform is performed to ground latitude/longitude co-ordinates M from the positional parameter before taking the expectation, resulting in the position estimate

C Ni

xî “ Epxi|Ziq “ ηi,j Mppi,jq (4.44) j“1 ÿ C where pi,j is the p-component of µi,j. The advantage of using this estimator is the minimisation of expected error. However, in an application context the risk of using a BMSE estimator is thatx î may correspond to an off-road position, since the raw weighted average ground-position does not take into account road constraints. This is often relevant when the vehicle trajectory is ambiguous, for instance in the vicinity of a junction, or when GNSS measurements have been reduced. For output purposes, it is possible to rationalise an off-road BMSE estimated position with the network via a minimum-weighted-distance map-matching process.

Another method of computing position estimates from Pˆpxi|Ziq is to use the maximum weight estimator (MWE):

MAX xˆi “ Mppi q (4.45) where MAX pi “ ppi,j : ηi,j “ maxtηi,j,uq (4.46) j This approach estimates the vehicle’s true position to be the position at the centre of the highest-weight cluster component, and is therefore necessarily an on-road position. This is an advantage of such an approach compared to the BMSE estimator, particularly at junctions or trajectory-ambiguous circumstances. However, this ambiguity

92 will not be reﬂected or communicated to the user using the MWE estimator.

A third approach may be to use the MWE in trajectory-unambiguous circumstances, but use a multiple-output approach in ambiguous scenarios. This may include (for example) user-communicated output which consists of 2 estimated positions which are of similar likelihood at ambiguity-high periods. The output should be vividly colour-coded to rank estimates clearly by relative likelihood to minimise confusion for the user, and limited to a small number of output positions (e.g. 2 or less). This may alert the user to the ambiguity and not place excessive trust in a single estimate.

Figure 4.8: Flow diagram of the computational steps in the GCAF’s iterative data fusion/estimation process. This process represents the recursive approximate solution to equation 2.1 used in position estimation.

93 4.3 Conclusions

This chapter has introduced the Gaussian Cluster Approximate Filter (GCAF), a new statistically optimised solution to the problem of low-cost urban vehicle positioning using only GNSS and road map data. The formulating principles were based on both empirical and theoretical evaluations of performance-impact factors of existing positioning solutions, and the design focusses on maximally improving performance in positioning-hostile environments, conducted in chapters 2, and 3. The key new elements are a combination of an approximate real-time recursive solution to the Bayesian positioning equation 2.1 using truncated Gaussian moment matching and the PSC approach to integrating road constraints into positioning (see section 2.2.2 in chapter 2 for a deﬁnition of PSC methods). These elements ensure closeness to the mathematically exact solution to equation 2.1 which is a demonstrated error-reducing feature based on methods evaluated in the literature, and in the performance evaluations in chapter 3

The main structural elements of the GCAF include

• An eﬃcient time-evolving Gaussian cluster uncertainty representation Pˆpxi|Ziq of the vehicle state xi at time ti • A multiple-motion-state vehicle dynamical model integrating road network positions, segment connectivity, turn restrictions and one-way streets

• A novel method for integrating non-standard auxiliary data such as traﬃc signal locations • A piece-wise locally linear GNSS measurement model for integrating both code-based GNSS pseudoranges and single-point-position measurement types that incorporates road network constraints • A novel method for handling non-linear segment boundary probability-distortion eﬀects using truncated Gaus- sian moment-matching

The GCAF positioning solution design is based on rigorous original empirical and theoretical evaluations of positioning method design impacts on performance, particularly in common positioning-hostile environments. As a result, it is expected that a reduction in positioning bias errors that harm accuracy and continuity in existing solutions will be observed, particularly in positioning-hostile environments. This prediction is empirically tested against reality in the next chapter by evaluating the performance of the GCAF in a range of common and challenging application scenarios, using both real and synthetically generated data.

94 Chapter 5

Positioning Performance Evaluation of the Gaussian Cluster Approximate Filter

5.1 Introduction

This chapter presents an evaluation of the GCAF low-cost positioning algorithm introduced and formulated in chapter 4, and an analysis of the key findings. Over a series of performance tests the GCAF is compared to existing low-cost positioning methods commonly in use over a broad range of common urban positioning environments (see chapter 2 for definition), using the positioning error time series as the main performance metric. These tests include positioning evaluation based on randomly simulated vehicle journeys through a real road network and simulated noisy GNSS, as well as tests based on real data (GNSS measurements collected during a real vehicle excursion, covering a range of geographic areas with different impacts on positioning). This two-part design of the performance evaluation has been developed to be robust; that is, to maximise the breadth of vehicular scenarios under which the GCAF is tested (including both friendly and hostile for positioning), in terms of the road network complexity, traffic conditions/vehicle motions/manoeuvres and GNSS visibility levels. Large-sample synthetic, randomly generated GNSS datasets with randomised trajectories in part 1 create a statistically significant evaluation dataset which can simulate a broad range of environments and eliminate possible selection biases due to conveniently selected starting points / trajectories (as noted by Quddus in [42], these biases are observed in past evaluation designs). Performance evaluations based on real GNSS data in part 2 capture nuances in vehicular motion, GNSS fluctuations and traffic conditions not necessarily fully captured in the simulations in part 1, which are important for the robust usability testing of the GCAF positioning algorithm. The broad design of these tests (both parts 1 and 2) and associated performance results are summarised in the figure 5.21. The robust design addresses identified shortcomings in performance evaluation methodologies and datasets in the literature (see chapter 2) including performance measure biases/statistical significance issues resulting from small samples (experiments using only one sample journey, or very short-duration journeys), narrow sets of evaluation parameters used in testing as well as potential “environment selection bias” where scenarios were selected which may favour the algorithms under evaluation (see [42]).

95 5.2 Experimental design and results of the GCAF positioning performance tests 5.2.1 Performance evaluation methodology and challenges The aim of the experimental design of the GCAF performance evaluations is to address existing methodological challenges identified in the literature (see chapter 2) including parameter selection confirmation-biases, statistical significance issues and breadth of test scenarios. The performance evaluation adopts a two-part approach to optimise the trade-off between limited resources (time, personnel and apparatus) and the necessary statistical significance/robustness of the evaluation results. Both parts will be applied to a range of existing and commonly- used positioning algorithms to allow for a meaningful comparison of common methods and the GCAF. The two parts are complementary and aim to produce a final performance dataset that is both statistically significant (large number of randomised simulation trials) and robust to the nuances of vehicular environments whose positioning impacts are difficult to model in simulations. Firstly, a GCAF performance test based on synthetically generated GNSS and vehicle trajectory data aims to test a broad range of common urban vehicle scenarios with resource-efficiency. The randomised vehicle trajectories, GNSS-measurements and resulting computed positions can be quickly generated based on physically validated models providing a large dataset for statistical significance of results [27, 26, 12]. Randomised trajectories are constrained to a road map based on the real Melbourne road network to allow representative sampling of a range of network features (e.g. turns, roundabouts, densely spaced blocks, multi-lane roads, junctions). The synthetic trials are divided into those which are GNSS-friendly (high visibility) and those which simulate urban canyons (poor GNSS measurement quality). The randomised start-points and speed/turn choices eliminate any environment-selection bias, such as selecting simpler routes which may over-estimate positioning performance. These biases have been identified and discussed in the literature survey (see section 2.3.1 in chapter 2) Secondly, a series of GCAF performance test based on real GNSS data and vehicle trajectories is performed to allow robustness of results to difficult-to-model phenomena that are not necessarily controlled for in the synthetic trials. Such phenomena can include unusual or particularly complex vehicle motion/traffic patterns, irregular GNSS measurement patterns and satellite visibility variations in urban canyons. A set of trajectories based on subsets of a single vehicle excursion dataset in inner-northern Melbourne will be examined (see figure 5.4). The scope of these tests will include variations in satellite visibility, complex network topology, impacts of modeling traffic signals and of using SPP (“single point position”) versus pseudorange type GNSS measurements. Both parts of the performance evaluation will use positioning error as a base metric, comparing the estimated positionsx î from the ground-truth position xi at time ti, generating error time series over the duration of the various trajectories (simulated or real). Accuracy and continuity of positioning will be evaluated for all positioning algorithms based on their error time series.

5.2.2 GCAF Performance test design based on batch Monte-Carlo simulations (Part 1) Part 1 of the GCAF performance evaluation compared the positioning error of the GCAF with existing positioning methods in a series of vehicle positioning simulations. Two datasets were synthetically generated in each test scenario - a dataset of 500 vehicle trajectories (denoted X) and a GNSS measurement dataset estimating those trajectories, simulated by randomly adding noise to X (the result denoted Z), both generated stochastically from probability distributions. All algorithms measured were fed identical input datasets (R,Z) from which to estimate positions, where R is the road network data expressed as a list of road segment shape-points in 2-d coordinates, and how they are connected (contained in an ”adjacency matrix”). Positioning algorithms evaluated for comparison include the Kalman Filter (KF), map-matched Kalman filter (MM-KF), interacting-multiple-models Kalman filter (IMM) and the map-matched IMM Kalman filter (MM-IMM). Errors i were computed by differencing the synthetically generated ground-truth vehicle trajectory xi P X with the estimated positionsx î computed by each method at each sampling time ti. Each error time series computed this way for a single simulated ground-truth trajectory is referred

96 to as a “trial”, of which 1000 were conducted and then error data aggregated for each positioning algorithm. Five hundred of the trials were simulated with steady moderate-good satellite visibility levels (SatVis=5, where in this chapter “SatVis” refers to the number of GNSS satellites in direct line-of-sight of the receiver), and the other 500 contained a 10 second full GNSS signal outage (SatVis=0) in the middle of the trajectory, with steady SatVis=5 before and after the outage.

Part 1 tests - Simulation models and parameters for generating X and Z datasets The synthetic “ground truth” trajectories were generated using the linear vehicle motion model 5.1 and assumed to be road-constrained. The road maps were based on real network data from OpenStreetMap and assumed to be of zero-width linear-segmented form with no road map error artificially introduced into the map data nor assumed by the estimation models. The road network was therefore represented as a graph where point-nodes represent shape points or intersections of roads, and connections between them represent the centrelines of lanes or roads. Estimatesx î were computed from these road constraints and synthetically generated GNSS-SPP measurements from the generating model (equation 5.1). Each single simulated ground-truth trajectory was generated using a linear Markov stochastic model (equation 5.1 beginning at a random start point on the road map (all points having equal chance of selection). The Markov model propagates the state vector xi “ ppi, viq where pi is the network-location parameter and vi is the along- road speed at time ti, as defined in section 4.1.4 in chapter 4, in the “Road constraints and along-road position co-ordinates” subsection.

xi`1 “ F xi ` wi (5.1)

Equation 5.1 is used to compute the subsequent state if the states xi and xi`1 share the same segment positionally, that is tpiu “ tpi`1u. If not, then a segment transition has occurred between times ti and ti`1, and the state xi`1 is determined with an additional protocol which considers topologically connected segments and selects a direction at random from the other segments radiating from the junction, with each direction having equal probability of selection. The time-invariant trajectory propagation matrix is given by

1 δt F “ 0 1 ˆ ˙ , where δt ” ti ´ ti´1 “ 1s is the time-invariant sampling period. wi is a Gaussian zero-mean white noise with time-invariant covariance

E T 0 0 Q ” pwi wiq “ 2 0 σv,SIM 2 ˆ2 ´2 ˙ , where σv,SIM “ 10m s is the dynamical process noise intensity used in synthetic trajectory generation. The trajectory is then given by xi “ Mppiq, where M is deﬁned in section 4.1.4 in chapter 4, in the “Road constraints and along-road position co-ordinates” subsection GNSS SPP positions zi were generated synthetically from the true trajectory xi using a linear stochastic model:

zi “ xi ` ωi (5.2)

, where ωi is a Gaussian zero-mean white noise with time-invariant covariance

E T 2 R ” pωi ωiq “ σz,SIM I2ˆ2 (5.3) 2 2 and σz,SIM “ 10m is the GNSS-SPP measurement noise intensity used in measurement generation.

97 Part 1 tests - Constraint-free Kalman Filter Estimation Parameters

The unconstrained linear-Gaussian Kalman Filter estimated the synthetically generated vehicle positions pxî, yîq as a subset of its state vector which was specified to be 4 dimensional including vehicle position and velocity x y x “ pxi, yi, vi , vi q. The estimation framework is described in further detail in [27, 25], and uses the standard minimum-posterior-square-error (“optimal Kalman gain”) approach applied to the predicted state residuals to generate output:

` xˆi “ xi ` Ki θi (5.4) where

x y • xî “ pxî, yî, vî , vî q is the state estimate at time ti ` • xi “ F xî´1 is the Markov-propagated (“predicted”) state

• xˆ0 is the initial state estimate at time t0, by default set to be unbiased (equal to the mean of the trajectory- generation distribution at time t0) 1 0 δt 0 0 1 0 δt • F “ ¨0 0 1 0 ˛ ˚0 0 0 1 ‹ ˚ ‹ is the˝ Markov state-propagation‚ matrix

` T ` T • δt ” ti ´ti´1 “ 1s is the sampling period Ki “ Pi H pHPi H `Rq is the Kalman gain (when good GNSS conditions are simulated)

• Ki “ 0 by default during simulated GNSS blockage

` T • Pi “ FPi´1 F ` Q is the propagated state covariance matrix ` • Pi “ pI ´ Ki Hq Pi is the GNSS measurement-updated state covariance matrix 25 0 0 0 0 25 0 0 • P “ is the initialised state covariance (prior uncertainty) at initial time t 0 ¨ 0 0 10 0 ˛ 0 ˚ 0 0 0 10‹ ˚ ‹ ˝ ‚ 0 0 0 0 0 0 0 0 • Q “ ¨ 2 ˛ is the modeled process noise covariance 0 0 σv,SIM 0 ˚0 0 0 σ2 ‹ ˚ v,SIM ‹ ˝ ‚ 1 0 0 0 • H “ is the GNSS-SPP measurement matrix 0 1 0 0 ˆ ˙ 2 • RSIM “ σz,SIM I2ˆ2 is the modeled GNSS-SPP measurement noise covariance

` • θi “ zi ´ H x is the measurement residual

The map-matched KF generated strictly road-constrained vehicle position by evaluating the road network position which minimised the the standardised (position-covariance-weighted) distance to the un-matched KF estimate described above.

98 Part 1 tests - Interacting-Multiple-Model Kalman Filter (IMM-KF) Test Parameters The interacting multiple model Kalman Filter (IMM-KF) used the same state space and linear-Gaussian minimum- square-error estimation framework as the KF (see preceding subsection), and is an application of the general method introduced by Blom and Bar Shalom in 1988 [6]. The IMM-KF is equivalent to a series of 3 Kalman Filters running in parallel with each integrating a diﬀerent vehicle motion model with the GNSS measurements zi to generate a series of estimated positions pxˆ1,i, xˆ2,i, xˆ3,iq at time ti. The estimated positions are then weighted-averaged based on the residual likelihoods. The weights of the parallel Kalman Filters evolved based on a Markov model matrix M encoding the probabilities of the motion state “switching” over time (see equation 5.7. The motion models of the 3 parallel KFs correspond to three motion states: “cruising”, “maneuvering” and “stopped”, corresponding respectively to the motion state indices (MSIs) “1”, “2” and “3”. Applying minimum posterior-square-error:

xˆi “ wi,j xˆi,j (5.5) j“1 ÿ is the IMM-KF state estimate and

3 T Pi “ wi,j Pi,j ` pxî,j ´ xîqpxî,j ´ xîq (5.6) j“1 ˆ ˙ ÿ is the estimated state covariance, where

3 • xˆi,j “ k“1 wi,j,k xˆi,j,k is the “mixed” state estimate for the motion state indexed j at time ti

• pxî,j,k,řPi,j,kq are the state and covariance estimates of parallel Kalman Filters (k “ 1, 2, 3) with input parameters Fk, Qk, Hk Rk, Pi´1,j and xî´1,j 1 0 δt 0 0 0 0 0 0 1 0 δt 0 0 0 0 • F1 “ ¨ ˛ Q1 “ ¨ 2 ˛ 0 0 1 0 0 0 σv,1 0 ˚0 0 0 1 ‹ ˚0 0 0 σ2 ‹ ˚ ‹ ˚ v,1‹ ˝ ‚ ˝ ‚ 1 0 δt 0 0 0 0 0 0 1 0 δt 0 0 0 0 F2 “ ¨ ˛ Q2 “ ¨ 2 ˛ 0 0 1 0 0 0 σv,2 0 ˚0 0 0 1 ‹ ˚0 0 0 σ2 ‹ ˚ ‹ ˚ v,2‹ ˝ ‚ ˝ ‚ 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 F “ Q “ 3 ¨0 0 0 0˛ 3 ¨0 0 0 0˛ ˚0 0 0 0‹ ˚0 0 0 0‹ ˚ ‹ ˚ ‹ ˝ ‚ ˝ ‚ are the dynamical state-specific propagation model matrices

2 2 ´2 2 2 ´2 • σv,1 “ 10m s and σv,2 “ 100m s are the process-noise intensities for the “cruising” and “maneuvering” dynamical states respectively.

• Hk “ H and Rk “ R are the motion-state-invariant measurement model matrices and are equal to the KF counterparts speciﬁed in the previous section (“Constraint-free Kalman Filter Test Parameters”)

pj,kq ´1 T ´1 • wi,j,k “ M wi´1,j Si,j,k exptθi,j,k Si,j,k θi,j,ku are the parallel Kalman Filter measurement-updated weights

99 ˆ T • Si,j,k “ Hk Pi,j,k Hk ` Rk is the residual covariance of parallel ﬁlter k of initial dynamical state j at time ti ˆ T • Pi,j,k “ Fk Pi´1,j Fk ` Qk is the predicted state covariance of parallel ﬁlter k of initial dynamical state j at time ti

` • θi,j,k “ zi ´ Hk xi,j,k is the measurement residual of of parallel ﬁlter k of initial dynamical state j at time ti

` • xi,j,k “ Fk xˆi´1,j is the predicted state of parallel ﬁlter k of initial dynamical state j at time ti

1 3 • P “ w P `pxˆ ´xˆ qpxˆ ´xˆ qT is the combined covariance estimate of the Kalman i,k w i,j,k i,j,k i,j,k i,k i,j,k i,k i,k j“1 ÿ ˆ ˙ Filters outputs with matching dynamical state k at time ti (take note of the summation index and indexes of relevant parameters)

• 0.7 0.29 0.01 M “ 0.4 0.3 0.3 (5.7) ¨0.05 0.45 0.5 ˛ ˝ ‚ is the dynamical-state Markov switching probability matrix, where Mpi,jq is the time-invariant probability of transition from state i to j at a any given sampling time.

3 • wi,k “ wi,j,k is the combined weight of state k following measurement update at time ti (note the index j“1 switch fromÿ equation 5.5 for notational clarity)

The map-matched IMM-KF generated strictly road-constrained vehicle positions by evaluating the road network point which minimised the the standardised (position-covariance-weighted) distance to the un-matched IMM-KF estimate described above.

Part 1 tests - GCAF Estimation Parameters

The GCAF used a 4-d network-local state space xi “ ppi, vi, hi, ∆tiq as speciﬁed in section 4.1.3 in chapter 4. The GCAF position estimate is computed as a minimum posterior-square-error position based on the Gaussian cluster posterior representation Ppxi|Ziq at time ti (see chapter 4 section 4.2)

xî “ wi,j Mppi,jq (5.8) j“1 ÿ where the ground long/lat/alt (LLA) position estimate outputx î “ Mppiq is generated by applying a “network- location” co-ordinate transform M to the estimated network parameter pi (see section 4.1.4, subsection “Road constraints and along-road position co-ordinates” in chapter 4 for definition). The iterative computation of cluster component weights wi,j and locations pi,j is discussed in detail in section 4.2 in chapter 4. The state space and all models and parameterisations used by the GCAF for positioning in this performance test are as detailed in chapter 4 section 4.2. Specific parameter values used in the performance tests (as defined in section 4.1.4 in chapter 4) are listed below.

2 2 • σz “ 25m

2 2 ´2 • σv,1 “ 10m s

2 2 ´2 • σv,2 “ 100m s

100 2 • σh “ 0 since altitude is redundant in SPP-measurement based ground positioning 2 • σ∆t “ 0 since receiver clock error is redundant in SPP-measurement based ground positioning • The Markov dynamical state switching matrix M used in the simulation-based performance tests is the same as in the IMM-KF case described above. • Turn probabilities (cross-junction component-propagation splitting weights) are assumed to be uniformly distributed across all allowed trajectories

• Disallowed trajectories from a junction (e.g. due to turn restrictions or one-way designations) are assigned 0 probabilities (0 weight multiplier)

Part 1 tests - Positioning Performance Test Results

Figure 5.1: Batch-averaged positioning error time-series results of the simulation-based positioning performance tests (Part 1) in GNSS-friendly conditions

101 Figure 5.2: Batch-averaged positioning error time-series results of the simulation based positioning performance tests (Part 1) in GNSS-hostile conditions

Figure 5.3: Batch-averaged positioning peak-error of simulation based tests (part 1)

102 5.2.3 GCAF Performance test design based on real GNSS-SPP measurement data (Part 2) Part 2 of the GCAF performance evaluation used real data collected during a vehicular excursion. Part 2 is further divided into parts 2a and 2b, comprising evaluations based on real GNSS SPP data, and further evaluations based on real GNSS pseudorange data, respectively. This is done to robustly test the GCAF’s capability to position using two types of GNSS input data types. Similar to part 1, the positioning performance measure was the positioning error computed by differencing a ground truth position xi from the estimatesx î of the GCAF and a range of other existing positioning methods. The methods compared with the GCAF vary between the tests; these are described in the relevant subsections and in the experiment results summary (figure 5.21). This was done across a range of test scenarios selected from the real dataset to represent a range of common positioning environments, described in the subsequent sections, with a performance dataset based on the positioning error time series produced in each test scenario.

103 Figure 5.4: Top: Performance test zones (circled) by test type, with GNSS-SPP dataset superimposed onto OSM road map. Bottom: Google Earth image of performance test regions seen above (University of Melbourne/Carlton Gardens precinct).

104 The selected test scenarios are subsets of the full vehicle excursion dataset and include three “test zones” as seen in figure 5.4. The input GNSS datasets for the positioning algorithms was the subset of the full GNSS dataset collected over the excursion which overlapped with the times that the vehicle was in the test zones. The ground-truth positions xi used in computation of position estimation error in the test zones were DGPS network- segment-centreline-projected positions (see figure 5.5). Two performance tests were conducted in part 2a and are discussed in the subsections below. The first measures positioning accuracy/continuity of a range of algorithms in a topologically/geographically complex road network environment, and the second evaluates the performance impacts of integrating traffic signal data with road maps and real GNSS-SPP into positioning.

Figure 5.5: An example two-lane road is modeled as connected piece-wise linear segments representing the road’s centre-line. For error computation, the true trajectory is projected onto the centreline model and compared with the along-road (i.e. along-centreline) position estimates as computed by each algorithm.

105 Part 2a Haymarket - Positioning using SPP in a topologically complex road network In this experiment positions were computed using GNSS-SPP and road map data inputs in Haymarket roundabout (see figure 5.6, circled in red within the zoomed-out figure 5.4). The Haymarket location was chosen because 1) satellite visibility was abundant (allowing for centimetre-level precision of ground-truth positions) and 2) there were other positioning hostile factors present, such as unusual road geometry/topology and sudden accelerations due to traffic patterns/signals were present to test their positioning performance impacts. The Haymarket performance test compared positioning accuracy/continuity performance of the GCAF, particle filter, Kalman Filter, IMM and road constrained variants over a 30 second positioning time-window within the testing zones. A complete 7-second GNSS outage was simulated near the middle of the 30-second positioning period (zero satellite visibility) upon entry to the roundabout to evaluate positioning performance across the GNSS outage in the presence of other hostile environments as well as testing the recovery following this event.

Part 2a Haymarket - Estimation parameters of the positioning methods tested The GCAF, Kalman Filter and IMM parameters are exactly as defined in section 5.2.2. The parameters of the particle filter are described below. The particle filter generates position estimatex î from an implicitly network-constrained P-V state space, using the network parameter and co-ordinate transform from network-to-ground M described in the GCAF parameters (see section 5.2.2, i.e. from xi “ ppi, viq to pxi, viq. The network parameter is estimated using minimum posterior- square-error approach, using a time-evolving weighted point-mass-cluster posterior distribution over the P-space, mapped to LLA ground co-ordinates in X-space and then weighted-averaged:

xˆi “ wi,j Mppi,jq (5.9) j“1 ÿ where

• ni ” n “ 500 is the time-invariant particle swarm size at time ti •M is a network-parameter-to-LLA co-ordinate map, as deﬁned in section 4.1.4 of chapter 4

• wi,j is the weight of particle j at time ti, updated iteratively according to the positional likelihood:

Λpzi|xi,jq wi´1,j wi,j “ n (5.10) j“1 Λpzi|xi,jq wi´1,j

th ř • xi,j “ ppi,j, vi,jq is the state of the j particle at time ti

th • pi,j is the network location of the j particle at time ti in network-local co-ordinates

th • vi,j is the along-road speed of the j particle at time ti

• xi,j is computed iteratively using the linear particle propagation model:

xi,j “ F xi´1,j ` wi (5.11)

1 ∆t • F “ is the particle state-propagation matrix when tp u “ tp u are on the same segment. otherwise 0 1 i´1 i ˆ ˙ the additional GCAF cross-segment position transfer protocol for Gaussian components applies when pi´1 ‰ pi .

106 • wi is a zero-mean Gaussian white noise term, with covariance 0 0 Q ” EpwT w q “ i i 0 σ2 ˆ v,PF˙

2 2 ´2 • σv,PF “ 25m s is the particle propagation process noise

• The particle swarm x0,j is initialised in an unbiased way by randomly generating a particle swarming (n “ 500) 2 with Gaussian distance N p0, σxq from the ground truth position x0 at t0 2 2 • σx “ 25m is the initial swarm dispersion

Part 2a Haymarket - Positioning Performance Test Results

Figure 5.6: Close-up images of the Haymarket real GNSS-SPP data based testing zone, Google Earth (top) with induced GNSS outage highlighted in the OSM model (bottom)

107 Figure 5.7: Visualised estimation outputs of various positioning algorithms from the Haymarket real GNSS-SPP data based performance tests during good GNSS conditions

Figure 5.8: Visualised estimation outputs of various positioning algorithms from the Haymarket real GNSS-SPP data based performance tests during hostile GNSS conditions

108 Figure 5.9: Time-averaged estimation errors of various low-cost positioning algorithms in the Haymarket real GNSS-SPP data based tests

Part 2a Carlton - Positioning with traffic signal location data integration (TSLDI) Two further tests using the collected GNSS-SPP were conducted in part 2a to evaluate the positioning performance impacts of traffic-signal location integration. The tests measured GCAF positioning error over two testing regions near Carlton Gardens (see figure 5.4) where traffic signals were present, comparing accuracy when the GCAF integrated traffic signal location data with the road map/GNSS to when it was not. Of interest was whether integrating known positions of traffic signals could predict and account for changes in motion in their vicinity, thereby reducing positioning biases/errors caused by unexpected/sudden motion changes. A similar analysis was carried out for a particle filter to compare the relative performance impacts. The first test (30 second test window, figure 5.12) was conducted near the the corner of Rathdowne/Grattan streets with a traffic signal at the corner, where the vehicle encountered a 12 second delay. The second test (27 second test window, see figure 5.14) included two signals along Rathdowne st: a pedestrian crossing signal at the intersection of Pelham/Rathdowne streets (no delay encountered) and a traffic signal at the Queensberry/Rathdowne st intersection (5 second delay encountered). GNSS visibility was moderate (4 satellites) so that the positioning performance impacts of traffic signal integration would not be completely masked by ideal GNSS conditions. How- ever a full GNSS outage was not induced since the positioning instability caused may not allow a meaningful or representative evaluation of the performance impacts of traffic signals as measured by positioning error.

Part 2a Carlton - Estimation parameters of the positioning algorithms tested The GCAF parameters for the signal data non-integrated test were the same as specified in section 5.2.2, including unbiased cluster initialisation with mean x0 with the same initial state covariance as section 5.2.2. The traffic-signal location data integrated (TSLDI) GCAF incorporated a modified motion model when the

109 vehicle entered any of the pre-defined “traffic signal proximity zones” (TSPZs). These were defined to be all points less than 30m along-segment distance from a network node (identified as a junction with traffic signals, see section 4.1.4, subsection “Traffic Signal Location Data Integration” in chapter 4 for detailed discussion of the GCAF-TSLDI model). The modified motion model included a TSPZ-specific dynamical-state switching probability matrix given by

0.5 0.4 0.1 MTSPZ “ 0.2 0.3 0.5 (5.12) ¨0.01 0.29 0.7˛ All other parameters of the GCAF-TSLDI were as detailed˝ in section 5.2.2.‚ The parameters of TSLDI-free particle filter were the same as detailed in section 5.2.2. The TSLDI-enabled particle filter had the same parameters as the TSLDI-free PF in non-TSPZ areas (the TSPZ definition is the same as for the GCAF above). Within TSPZs (defined in the same manner as for the GCAF), the TSLDI particle propagation model was modified to increase the stopping probability within the TSPZ. Specifically, a particle entering a TSPZ stops within the TSPZ if it is assigned a binary “stop” index IS with probability

tS pS “ (5.13) tS ` tG where tS and tG are the modeled durations of the traffic signal sequences for “stop” (late amber/red) and “go” (early amber/green). The chosen values were tS “ tG “ 30s giving pS “ 0.5. A particle assigned IS “ 0 in a TSPZ ignores the traffic signal and propagates through the current TSPZ using the TSLDI-free stochastic model (equation 5.11). A particle assigned IS “ 1 stops at a randomised location within the TSPZ, sampled from a triangular distribution over the TSPZ region with mode at the signal location (see figure 5.10). The signal delay tD assigned to the particle is randomly sampled from a uniform Rp0, tSq distribution (see figure 5.10). The weight of the particle is still measurement-updated over the signal delay waiting period according to equation 5.10. After t D sampling periods the particle resumes propagation according to the linear stochastic Markov model (equation δt 5.11)Z ^ until it encounters another TSPZ.

110 Figure 5.10: Probability models used by PF in Part 2a Carlton test Top: Probability distribution of vehicle-stoppage-distance from red traffic signal used by the particle filter within the TPSZ. Bottom: Probability distribution of traffic signal delay duration over where tS is the maximum delay length used by the PF when encountering late amber/red signal.

111 Part 2a Carlton - Positioning Performance Test Results

Figure 5.11: Testing zones and traﬃc signal locations in the real GNSS-SPP/TSLDI positioning performance impact evaluation experiments (Part 2a Carlton)

Figure 5.12: Visualised positioning output of PF and GCAF based on real GNSS-SPP/TSLDI data (Grat- tan/Rathdowne st test)

112 Figure 5.13: Positioning error time series results from the Grattan/Rathdowne St real GNSS-SPP/TSLDI positioning performance impact tests

Figure 5.14: Visualised positioning output of PF and GCAF based on real GNSS-SPP/TSLDI data (Queens- berry/Rathdowne st test)

113 Figure 5.15: Positioning error time series results from the Queensberry/Rathdowne St real GNSS-SPP/TSLDI positioning performance impact tests

Figure 5.16: Time-averaged positioning error results of the real GNSS-SPP/TSLDI positioning performance impact tests

114 Part 2b tests - GCAF Performance test design based on real GNSS code-pseudorange measurement data Two real-data positioning performance tests were conducted evaluating GNSS code-based pseudorange measurement/road map fusion, comprising part 2b of the GCAF performance evaluation. The aim of these tests was to evaluate performance of the GCAF’s pseudorange positioning capability as well as test it in the common GNSS- reduced environments (0ăSatVisă4) that were not total blockages (by contrast to the simulation-based and real SPP measurement data based tests). The importance of positioning from pseudoranges is that in reduced GNSS environments, positioning methods estimating from pre-processed SPP measurement inputs will have no measurement when SatVisă4 since 4 is the minimum visibility to achieve SPP positional lock [25]. This means that the existing sparse GNSS data is underutilised and the accuracy can be significantly degraded compared with methods that can in some way use the ă 4 pseudoranges available. It is therefore important to test this capability which is necessary in optimal low-cost urban positioning. Positioning accuracy and continuity performance were evaluated using positioning error as a base metric, with DGPS used as a ground-truth. For comparison, positioning output was also generated from an extended Kalman filter (EKF) and map-matched (MM)-EKF using the same GNSS and road map data inputs. The ground-truth was computed using network-segment-projected DGPS for along-road error computation in road-constrained algorithms (MM-EKF and GCAF) (see figure 5.5), and un-projected DGPS used in error calculations for the constraint-free EKF. The Carlton Gardens precinct testing zone (see figure 5.17) was chosen due to its high satellite visibility levels which provided accurate ground-referencing for error computation via DGPS. The testing zone also provided a broad range of vehicular environments, such as variable road network topology (intersections, multi-lane streets, dense roads and lane blocks) and a high-frequency maneuver/stoppage environments due to the typical traffic conditions and the choice of trajectory (right turn at Rathdowne st). The GNSS-deprived positioning environments were simulated by artificially reducing the set of visible satellites to 2, with satellite IDs selected at random and remaining fixed throughout the induced GNSS outage. Two tests were conducted within the Carlton Gardens area, where the start/end times of the artificially induced SatVis=2 GNSS partial-outage were different over each test (44 seconds from Lygon st to Rathdowne st along Grattan st and 30 seconds from Drummond st to Rathdowne st along Grattan, see figure 5.18).

Part 2b tests - Extended Kalman Filter Estimation Parameters The Extended Kalman Filter is an extension of the linear-Gaussian Kalman Filter described in section 5.2.2 (subsection “Constraint-free Kalman Filter Test Parameters”) to allow non-linear measurement/dynamical model integration by applying a linear-Gaussian models to the models that are ﬁrst linearised. The EKF is necessary to integrate pseudorange measurements since the standard measurement model (equation 6.3) is non-linear and is therefore linearised in equation 5.16. The map-matched (MM-)EKF matches the EKF output with a road map to ensure road-constrained positioning output. The EKF and map-matched EKF (MM-EKF) estimated positions are based on a 6-d state vector xi “ x y x y pxi, yi, vi , vi , hi, ∆tiq where hi is the vehicle’s altitude above sea-level, xi, yi, vi and vi are as deﬁned for the Kalman Filter in section 5.2.2 and ∆ti is the receiver’s clock bias (in seconds). GNSS pseudorange measures were integrated using the linearised Kalman gain on the predicted state residuals, based on the parameters below:

` xî “ xi ` Ki θi (5.14) where x y ˆ ˆ • xî “ pxî, yî, vî , vî , hi, ∆tiq is the state estimate at time ti ` • xi “ F xî´1 is the Markov-propagated (“predicted”) state

• xˆ0 is the initial state estimate at time t0, by default set to be unbiased (positional component is equal to the initial DGPS ground-truth position x0 and altitude h0)

115 1 0 δt 0 0 0 0 1 0 δt 0 0 ¨0 0 1 0 0 0˛ • F “ ˚0 0 0 1 0 0‹ ˚ ‹ ˚0 0 0 0 1 0‹ ˚ ‹ ˚0 0 0 0 0 c‹ ˚ ‹ is the˝ time-invariant Markov‚ state-propagation matrix

• δt ” ti ´ ti´1 “ 1s is the sampling period

` T ` T • Ki “ Pi Hi pHi Pi Hi ` Riq is the linearised Kalman gain ` T • Pi “ FPi´1 F ` Q is the propagated state covariance matrix ` • Pi “ pI ´ Ki Hiq Pi is the GNSS measurement-updated state covariance matrix 25 0 0 0 0 0 0 25 0 0 0 0 ¨ 0 0 10 0 0 0 ˛ • P “ 0 ˚ 0 0 0 10 0 0 ‹ ˚ ‹ ˚ 0 0 0 0 10 0 ‹ ˚ ‹ ˚ 0 0 0 10 0 10{c2‹ ˚ ‹ ˝ ‚ is the initialised state covariance (prior uncertainty) at initial time t0 • c is the speed of light 0 0 0 0 0 0 0 0 0 0 0 0 ¨0 0 σ2 0 0 0 ˛ • Q “ v,EKF ˚0 0 0 σ2 0 0 ‹ ˚ v,EKF ‹ ˚0 0 0 0 σ2 0 ‹ ˚ h,EKF ‹ ˚0 0 0 0 0 σ2 ‹ ˚ t,EKF ‹ is the˝ time-invariant modeled process noise covariance‚

2 2 ´2 • σv,EKF “ 10m s is the EKF acceleration process noise

2 2 • σh,EKF “ 1m is the EKF altitude-change process noise

2 2 • σt,EKF “ p10{cqm is the EKF receiver clock error-drift process noise

∇T fpxR, xS q R i,j R S pxi,0,xi,1q . ˇ • H ” ¨ . ˇ ˛ i . ˇ ˚ ‹ ∇T fpxR, xS q ˚ R i,j R S ‹ ˚ pxi,0,xi,SatV is q‹ ˚ ˇ i ‹ ˝ ˇ ‚ is the measurement matrixˇ in ECEF co-ordinates B • ∇R “ is the ECEF receiver-state gradient vector BxS ˆ i ˙ •

j R S ρi ” fpxi , Xi q ` wi,j

116 S R 2 S R 2 S R 2 “ pxi,j ´ xi q ` pyi,j ´ yi q ` pzi,j ´ zi q ` c ∆ti ` wi,j (5.15) b is the non-linear pseudorange measurement model with additive noise • ` zi ´ zi,0 “ Hi pxi ´ xi q ` wi,z (5.16) is the linearised measurement model with additive Gaussian measurement noise

R R R R • xi “ pxi , yi , zi q is the receiver position in ECEF co-ordinates S S S S • Xi,j “ pxi,j, yi,j, zi,jq is the position of satellite j at time ti in ECEF co-ordinates

j pj,jq • wi,j (the signal white-noise in measurement ρi ) is a zero-mean Gaussian random variable with variance Ri σ2 σ2 • R “ diag ρ , ... , ρ is the modeled GNSS-pseudorange measurement noise covariance i sin θ sin θ ˆ i,1 i,SatV isi ˙ R ECEF ` • xi,0 ” ΦLLA pxi q is the EKF linearisation point

ECEF • ΦLLA pxq maps a point x in LLA co-ordinates to the same position in ECEF co-ordinates

• θi “ zi ´ z0,i is the measurement residual

1 SatV isi • z0,i “ pρ0,i, ..., ρ0,i q is the “reference measurement” at time ti

j R S • ρ0,i “ fpx0 , xi,jq are the “reference ranges” The MM-EKF generated positions using the same state space, models and parameters as listed above for the MM EKF, and then matched the positions to the road map by applying a matching function: xˆi “ ΦMM pxi, Pi,Mq where

• xi is the EKF state estimate output computed according to the above method and parameterisation

• Pi is the EKF state estimate covariance computed according to the above method and parameterisation • M is a set of road constraint data

• ΦMM pxq associates a ground-position x with the road constraints M, by identifying the point with minimum statistically weighted distance from x to M. The distance is scaled through weighting (multiplying the ´1 geometric distance by positional entries of Pi )

Part 2b tests - GCAF estimation parameters The models and method of pseudorange/road map fusion of GCAF positioning is detailed in sections 4.1.4 and 4.2 of chapter 4. A 4-d state space xi “ ppi, vi, hi, ∆tiq was adopted in estimation with pi and vi being the network-position and along-road speed, hi the above-sea-level altitude and ∆ti the receiver clock error at time ti. The estimation parameters of the GCAF were similar to the design in section 5.2.2, but with substitute matrices Fi, Qi, Hi and Ri to account for the augmented state space and pseudorange measurements. The appropriate matrix parameterisation for the dynamical model can be found in section 4.1.4, subsection “Stochastic dynamical model for hidden-Markov state propagation” in chapter 4. The speciﬁc values of the cluster propagation matrix entries are:

2 2 ´2 • σv,1 “ 10m s

117 2 2 ´2 • σv,2 “ 100m s 2 • σh “ 1m 2 2 2 • σ∆t “ 10{c m • c is the speed of light • Dynamical state switching matrix M is as deﬁned in section 5.2.2

• Turn probabilities (junction-split cluster weights) are assumed to be distributed uniformly over all allowed trajectory choices

Details of the pseudorange measurement model used by the GCAF can be found in the section 4.1.4, subsection 2 2 “Measurement model for code-based GNSS pseudorange measurements” in chapter 4, section. A value of σz “ 25m was used. The model linearisation, spatial co-ordinate handling and cluster/covariance propagation are adopted as deﬁned in section 4.2 in chapter 4.

118 Part 2b tests - Positioning Perforance Test Results

Figure 5.17: Close-up images of the Carlton Gardens precinct real GNSS-pseudorange data based testing zone (Part 2b), Google Earth (top) with corresponding OSM network-region model (bottom)

119 Figure 5.18: Visualised estimation outputs of various positioning algorithms in the real GNSS-pseudorange data based positioning performance tests (Part 2b). Top: low GNSS visibility (SatVis=2) region induced at Lygon St. Bottom: low GNSS visibility region (SatVis=2) induced at Drummond St

120 Figure 5.19: Positioning error time series results for various algorithms from the real GNSS-pseudorange data based performance evaluation experiments

121 Figure 5.20: Time-average positioning error results for various algorithms from the real GNSS-pseudorange data based performance evaluation experiments

5.2.4 Results summary Figure 5.21 includes a summary of the test results outlined in the preceding sections.

122 Figure 5.21: Results summary table - GCAF positioning performance evaluation tests

5.3 Key findings and analysis of the GCAF performance evaluation results 5.3.1 Key findings of the performance evaluation tests In this section the key findings of the GCAF performance test results are summarised. These, along with the new GCAF model, were presented as a conference paper at the UPINLBS 2014 conference in Corpus Christi, Texas [38].

Findings from the simulation based performance tests (Part 1) The experimental design and parameters of these GCAF performance tests is discussed in section 5.2.2. The results of the GCAF performance evaluation based on synthetically generated trajectory/GNSS measurement data yielded two key findings. Firstly, in good-GNSS environments, adequate accuracy and continuity was observed in all tested methods using standalone GNSS-SPP measurements (as expected). The GCAF had a slightly improved average accuracy (3.5m) compared with other positioning methods (e.g. 5m for the KF, see figures 5.1 and 5.3). Secondly, during the brief total signal-blockage simulations (10s with no GNSS data), the GCAF demonstrated a capacity to significantly contain positioning error growth and significantly more continuous positioning output, reaching a peak of only 7.5m (ă10m GCAF positioning errors over 70% of the GNSS outage period compared with 40% for the PF, 30% for the IMM-MM and 20% for other methods). The KF, IMM, MM-KF and MM-IMM positioning error grew at 2-3 m/s increasing to over 30m peak error over the blockage period, while for the PF it rose to 22m compared with only 12m peak error for the GCAF. The particle filter positioning error also grew, albeit at a steadier rate of 1-1.5m/s, thus still losing service after 3-5 seconds and showing slower recovery, causing positioning discontinuity. The measured service recovery delay of the particle filter following GNSS recovery was 1 second, compared with 3 seconds for other methods. These findings are reflected in the summary figures 5.2 and 5.3.

123 Findings from the real GNSS-SPP data based performance tests (Part 2a Haymarket) The experimental design and parameters of these GCAF performance tests is discussed in section 5.2.3. All positioning methods tested in the Haymarket tests demonstrated good accuracy and continuity performance in good GNSS conditions despite the adverse network topology/traffic factors. This corresponded with relatively accurate (ă10m peak error) positioning output through the roundabout (GCAF: 4m, PF: 5m, MM-IMM: 5.5m, MM-KF: 6.3m, see figures 5.7 and 5.9). This indicates that the Bayes-optimal road constraint integration methods (particle filter and GCAF) showed marginally better positioning accuracy than the covariance-weighted-distance map-matched solutions. The GCAF demonstrated significantly better performance than other methods in the simulated GNSS-deprived conditions than other methods compared (see figures 5.8 and 5.9). The average GCAF positioning error over the brief full GNSS outage was only 7.3m, below the peak threshold of 10m causing positioning discontinuity. This is contrasted with the PF (13m), MM-IMM (14m) and MM-KF (16.9m) which all breached this threshold, resulting in discontinuous and less accurate positioning. The GCAF best identified the true trajectory of the vehicle through the roundabout compared with the MM-KF and MM-IMM which generated the wrong trajectory output, partly explaining the higher peak error figures. The GCAF had the fastest recovery from peak error (1s compared with 4s for the MM-IMM and MM-KF to fall below 7m). The real GNSS-SPP measurement data based performance test results from the Haymarket roundabout are largely consistent with the findings of the simulation based results in part 1 in terms of the error characteristics of the methods tested.

Findings from the real GNSS-SPP and integrated traffic signal locations data based performance tests (Part 2a Carlton) The TSLDI performance evaluation found that TSLDI had relatively little measurable performance impact on GCAF positioning in all positioning environments tested within both testing regions (see figures 5.13, 5.15 and 5.16). There was a slight (but statistically insignificant) difference in average positioning error in the particle filter TSLDI-enabled and standard forms. The impacts of TSLDI ranged from slight increases to slight decreases in error i for both methods, and although the impacts were slightly greater in the PF they were still insignificant. The GCAF showed almost no variation between TSLDI and non-TSLDI forms over both testing regions (see figure 5.16). In the Grattan/Rathdowne st tests, the GCAF demonstrated a significant performance improvement over the PF both in the TSLDI and non-TSLDI cases. The particle filter performed worse during accelerations and vehicle turns, with error spiking at stops ahead of red traffic signals (where there was also a lower SatVis=3 visibility), which was dampened in the TSLDI case (see figure 5.13). This error behaviour is consistent with results from these positioning methods in Parts 1 and 2a Haymarket.

Findings from the real GNSS code-pseudorange data based performance tests (Part 2b) The experimental design and parameters of these GCAF performance tests is discussed in section 5.2.3. Two performance tests in the Carlton Gardens vicinity evaluated the accuracy and continuity performance of the GCAF, EKF and map-matched EKF (MM-EKF) over medium-to-long duration GNSS partial-outage (SatVis=2) environments (see figure 5.17). The results clearly demonstrated that the GCAF significantly improved positioning accuracy and continuity relative to other methods in GNSS-hostile conditions (see figures 5.19 and 5.20). GCAF positioning had 5.5m average positioning error in the first trial (case 1, compared with 30m/25m for the EKF/MM- EKF), and in the second trial (case 2) mean error was GCAF: 4.3m, EKF: 29.2m and MM-EKF 30.2m. The positioning output plots and positioning error time series plots (figures 5.18 and 5.19) show the relative containment of positioning error growth during the GNSS-reduced periods, and how very-high service continuity over even prolonged periods was provided (79% of the 44 second partial GNSS outage period under 10m in case 1, 87% of the 30 second partial GNSS outage in case 2). By contrast the road-unconstrained EKF and road- constrained MM-EKF demonstrated significantly poorer performance during the SatVis=2 periods, with sustained periods of steady positioning error divergence (visible in the output plots in figure 5.18) due to trajectory drift or

124 mismatch. This caused low continuity in positioning, with 95%/91% of EKF/MM-EKF position estimates above the threshold 10m in case 1 compared with 21% for the GCAF, and 73%/70% above 10m in case 2 compared to 13% for the GCAF. The growth in error for the EKF/MM-EKF was immediate following the GNSS visibility reduction resulting in service failure after only 3s/8s of SatVis=2 conditions in cases 1/2, and showing no ability to correct.

In case 1 (figure 5.18, above) the error in the EKF/MM-EKF steadily grows at more than 1m/s on average, then peaks at 61m/52m and drops whilst remaining well above 30m in absolute error (causing significant discontinuity). The temporary drop is possibly due in part to the vehicle stoppage at Rathdowne st creating a less ambiguous positioning environment by removing accelerations (see figures 5.19 and 5.18). However following the Rathdowne st turn the EKF/MM-EKF errors begins to diverge briefly likely due to the trajectory ambiguity and motion change. The recovery following GNSS-restoration to moderate SatVis levels is slow for both EKF and MM-EKF, taking 6-7 seconds to fall back to ă10m positioning error. By comparison, the GCAF successfully contains error growth and, apart from some temporary error spikes, maintains a stable error level despite the GNSS conditions, with a quick 2 second recovery to ă10m positioning error following GNSS restoration. In case 2, the EKF and MM-EKF generated position estimates which were unstable (error-unbounded) during the simulated GNSS outage, with error growing steadily following the GNSS reduction at 2m/s to a peak of 100m before GNSS was restored. Figure 5.18 reveals that the EKF/MM-EKF were unable to identify that the vehicle has stopped, and incorrectly identified the associated pseudorange signal change as a change in direction resulting in an incorrectly identified trajectory and error divergence. Following signal recovery, an 8 second delay to ă10m positioning error was observed. By contrast, the impacts of the stoppage and low GNSS visibility on the GCAF is significantly reduced, with only two low-severity and duration error spikes visible at t=15 and t=40. GCAF positioning recovery following GNSS-reinstatement is slower than in case 1, taking 5 seconds (still faster than the EKF/MM-EKF).

5.3.2 Analysis of key findings from the GCAF performance evaluation tests The key findings of the positioning performance test results described in section 5.3.1 provided a range of insights about the performance characteristics of the algorithms This includes the relative strengths and vulnerabilities of the GCAF in a range of vehicular environments compared with the other methods tested, as detected by patterns in the positioning error time series. This section highlights and analyses those patterns and attempts to explain these performance findings in terms of the mathematical features of the GCAF when contrasted with the other tested methods.

Analysis of simulation based performance tests (Part 1) The simulation based tests showed that the GCAF positioning solution provided improved accuracy and continuity both in GNSS-friendly and hostile conditions when a brief but severe GNSS outage was encountered (10 seconds with SatVis=0). The GCAF demonstrated an ability to utilise constraints to significant trajectory-correcting effect, and restore continuity in positioning over these GNSS outage periods. The Bayes-optimal method of integrating constraints ensured that critical topological and geometric information from the road network was fused in a maximally beneficial way that kept the trajectory stable and geographically continuous. The direct integration of topological (road connectivity) constraints into the dynamical motion model ensured physical feasibility was respected, and sudden “jumps” in position causing mismatch did not occur. This was not the case for other map-assisted methods such as the map-matched (MM) and IMM-MM Kalman Filters, which quickly diverged in positioning error to service-incapacitating levels causing discontinuity. This error divergence can be explained by the lack of topological constraint integration in the less sophisticated motion models, which caused the positions to be geometrically matched based on linear extrapolations, neglecting connectivity considerations and causing jumps across non-connected road segments. This effect is particularly noticeable during GNSS outages where the constraint information has more relative value to positioning than GNSS (see chapter 2 and 3). In GNSS-abundant environments, this effect is masked and all algorithms perform to similar accuracy/continuity levels.

125 The particle-filter, being a more Bayes-optimal positioning solution with respect to constraint integration, showed a greater mitigation of these effects. However, the accuracy was still lower than the GCAF and recovery time slower. This is due to the relatively statistically inefficient posterior representation of positional uncertainty (large-population weighted point swarm), compared with the Gaussian cluster of the GCAF. Due to the broader sampling of a Gaussian component, fewer components are necessary for the Gaussian cluster to effectively represent the often complex, multi-modal positional uncertainty distributions encountered near intersections and during GNSS outages. This means the particle swarm was less able to efficiently sample the posterior during the GNSS outage period (“swarm degeneracy”) causing estimation biases that reduced accuracy. The lower spread of sampling due to the localised nature of point-particles caused the positioning recovery time post-GNSS-recovery to be greater due to the poorer coverage of the network.

Analysis of real GNSS-SPP data based performance tests in GNSS-hostile environments (Part 2a Haymarket) The performance findings in the Haymarket test largely mirrored those of the simulation-based (Part 1) tests for all positioning methods tested, with the GCAF demonstrating superior accuracy/continuity in positioning hostile environments including GNSS outage and topologically complex regions/intersections. The immediately preceding section is referred to for an analysis of these findings in terms of the relative mathematical features of the GCAF and other methods, as the reasons are directly applicable to the Haymarket test. The similarity of results compared with the simulations shows the appropriateness of the models used in the simulations. The significant mismatch of the trajectory shown in figure 5.8 is a real-data example of the divergence effect observed in the results of the GNSS-outage simulation tests (reflected in figure 5.2), and is caused by sub-optimal integration of road topology into the motion models, which neglects the potential vehicle exit from the roundabout, resulting in linear forward- extrapolation along the roundabout. This is in contrast with the GCAF which correctly identifies the trajectory using the multi-hypothesis trajectory prediction approach to intersections.

Analysis of real GNSS-SPP data based performance tests with integrating traffic signal locations (Part 2a Carlton) The experiments measuring the positioning performance impacts of traffic signal location integration in moderate GNSS found no statistically significant impact to the particle filter or GCAF. While traffic signal locations were shown to have a slight but measurable impact on positioning accuracy (see figure 5.16), the information utility gain in the presence of road constraints and GNSS is minimal and in some instances increased positioning error. A performance impact was observed in the particle filter in the vicinity of the signals due to the modified motion model in the “proximity zones” as discussed in section 5.2.3. The impact of TSLDI on GCAF positioning accuracy was even less significant due to the inherent multiple-structure dynamical model which is already responsive to potential stoppages/accelerations near traffic signals, meaning minimal added utility was provided by precisely knowing traffic signal locations, even where SatVis was lower (SatVis=3 at the end of the Grattan experiment). The GCAF performed better than the particle filter in terms of accuracy at the Grattan/Rathdowne street intersection due to the broader probability sampling of the possible trajectories using split-Gaussian predictions (see chapter 4, section 4.2 for more details).

Analysis of real GNSS code-pseudorange data based performance tests (Part 2b) The medium to long duration induced partial GNSS outages in the real-data pseudorange based tests simulated the signal-masking effect of urban canyons. The main findings included the qualitative error convergence vs divergence in the GCAF vs map-matched EKF positioning algorithms during the induced medium/long GNSS partial-outages. In line with performance predictions based on the theoretical results of chapter 3, the GCAF contained error growth during the SatVis=2 conditions due to the effective constraint integration which increased the marginal utility of the sparse GNSS as a trajectory correction device, minimising segment mismatch and ensuring positioning continuity (as discussed in chapter 2, section 2.3 regarding “PSC methods”). This resulted in a “bridging” of the GNSS outage,

126 with positioning performance over the SatVis=2 period kept, on average, within 10m. The GCAF error time series had low-intensity and quick-recovery error spikes due to additive positioning-hostile factors such as intersection crossings (adding trajectory ambiguity) and dynamical variations such as quick stops/starts. The MM-EKF integrated road constraints but achieved only marginal performance benefit from the sparse GNSS due to the PSU road-constraint integration model that did not efficiently integrate topological information, resulting in frequent and significant mismatch over the GNSS outage period. This meant that GNSS drift over the SatVis=2 period was not effectively contained, and served to derail the trajectory, with map-matching providing little relief.

As a result, significant trajectory error was observed, causing steady and un-impeded error growth and discontinuous output. The EKF misinterpreted the SatVis=2 GNSS signal reduction as a change of heading (particularly visible in the second case, figure 5.18 bottom), and similarly misinterpreted the change in pseudorange patterns at the stoppage as a trajectory change. The GCAF’s probabilistic multi-hypothesis trajectory evaluation process allowed the sudden dynamical change (reflected in changes to the reduced pseudorange set) to be correctly identified as a vehicle stoppage, due to effective ambiguity resolution through optimal constraint integration as a means of testing trajectory hypothesis and eliminating false/impossible motion according to the network connections. This feature led to significant average-error reduction (order 85%, or 25m).

5.4 Conclusions

This chapter has introduced a performance evaluation framework for the GCAF and implemented this framework in a series of tests designed to subject the GCAF to a broad range of hostile but common positioning environments. The findings of the performance evaluation (sections 5.3.1 and 5.3.2) validate the GCAF method for positioning and make a strong case for its further development. The performance validation approach has been designed to overcome methodological shortcomings identified in section 5.2.1 as well as in chapter 2 such as lack of statistical significance, test parameter selection bias and narrow test scope. This has resulted in a robust performance result dataset from which meaningful key findings (discussed in section 5.3) are drawn. The findings obtained from this dataset provide support for some of the research hypotheses discussed in chapter 1 (in particular hypotheses 3 and 5 regarding the reduction of estimation biases due to integration of road constraints, and 4 with regard to auxiliary data sources). The findings of the positioning tests have demonstrated that a strict Bayesian-probabilistic approach to propagating the vehicle state uncertainties has the capacity to eliminate positional biases and mis-match, and lead to error stability and quick recovery from temporary error divergences due to effective network sampling. In particular, the GCAF has demonstrated a significant capacity to restore positioning accuracy and continuity in GNSS-deprived areas due to the Bayesian information-utility-maximising integration of trajectory-corrective road constraint data such as road segment locations, network connectivity and road features such as turn restrictions and traffic signal location data. The restoration and error reduction levels have been identified across all environments tested, and have exceeded all other candidate methods tested, in many cases by a significant amount. It is concluded that progress has been made toward the research objective of developing an efficient method of data fusion for hostility-robust road constrained vehicle positioning. In chapter 6 following, the broader implications of this chapter’s results in the context of urban vehicle positioning will be discussed, as well as future work required to achieve robust, fully continuous and accurate positioning in every urban environment. In very-low GNSS conditions (SatVis=0 and SatVis=1) there is no existing positioning solution which provides ă10m accurate using road map data and GNSS alone. The GCAF has provided a useful framework for data fusion with a proven capacity to significantly improve accuracy and continuity in these conditions compared with other methods. While it is not sufficient to achieve stable positioning everywhere, the purpose- designed flexibility of the GCAF means it is equipped to integrate other sensor data with GNSS and road maps and robustly tackle very-low GNSS environments, and these suggestions for future work are discussed in the final chapter.

127 Chapter 6

Conclusions

6.1 Conclusions from the research 6.1.1 Overview The chief purpose of the research was, in summary, to investigate the factors impacting vehicle positioning performance in urban areas, and to then use the ﬁndings of this investigation to optimise the development of a new low-cost and performance-tested positioning algorithm which addressed the concerns about existing methods and their performance. It is argued that the purpose and overall objective of this research, as stated above, have been achieved. This follows from the conclusions, as justiﬁed below in sections 6.1.2 and 6.1.3, that the research hypotheses have been supported and the individual research aims of this thesis have been achieved. The research aims and hypotheses are discussed in section 1.2. In section 6.1.2 the level of support for the underlying research hypotheses which have driven the methodology designed to achieve these research aims is discussed. Discussion on the levels of progress toward individual research aims is contained in section 6.1.3.

6.1.2 Validation of hypotheses based on key ﬁndings The following is a re-iteration of the list of research hypotheses from chapter 1:

The key ﬁndings from chapters 3 and 5 (particularly relating to the positioning error time series outputs) clearly demonstrated that vehicle positioning error is inﬂuenced by a set of key environmental factors which have distinct and unique impacts on positioning accuracy. This lends support for hypothesis 1. The calculations in chapter 3 show that the theory underpinning standard stochastic models for vehicle motion and GNSS signal errors

128 predict that, in a ”typical” journey of a vehicle through an urban area, the positioning error time series will not be uniform but exhibit ”signature patterns” where, for example, the vehicle is in an urban canyon, or is turning, accelerating or moving through an intersection. Combinations of these factors also had characteristic error profiles. This was reflected in the real-data based empirical error findings in chapter 5. The validation of this hypothesis was important in the design of the GCAF in chapter 4 which took into account these error profiles in its Bayesian estimation method and the choices of auxiliary data that were drawn as position estimation inputs. The key findings in chapters 3 and 5 as well as the literature review in chapter 2 (particularly relating to the comparisons between road-map assisted and un-assisted positioning error) clearly demonstrated that:

• Road map information has signiﬁcant, often critical, positive impacts on positioning error across a range of positioning environments and

• The method of integrating road map information with GNSS in position estimation was a strong inﬂuence on how much and what sort of impact the integration had on positioning performance. Certain classes of method tended to have greater levels of position bias error than other methods.

These findings therefore lend support to research hypothesis 2. Road maps were found to be of critical importance in reducing position error, if integrated in an effective way such as to minimise estimation bias error. Two classes of road constraint integration method, PSU and PSC (see chapter 2, section 2.2.2 were compared, finding that PSC tended to exhibit less bias error and lead to general levels of mean-error reduction, as well as contributing important qualitative performance improvements such as error stability (i.e. leading to uniform levels of error over time, rather than growing error). These benefits tended to be greater (in percentage terms) in more ”positioning-hostile” environments, where the utility of such error reductions was greatest in order to achieve positioning accuracy suitable for more C-ITS applications. This trend of utility maximisation in hostile environments is seen as an important finding for future work in recognising the high benefit-cost ratio of integrating road constraints effectively. The observation that the method of integrating road maps makes a significant difference to their positioning performance impact (which can, if done ineffectively, in fact be negative) is also seen as important to optimising low-cost methods in the future. The PSC method (as defined in chapter 2, section 2.2.2) is strongly advocated over the PSU approach based on this research, in order to obtain the significant and often critical improvements to positioning in hostile positioning environments that road maps have the potential to provide, as demonstrated by the findings of chapters 2, 3 and 5. Chapter 5 presented the output and key findings of robust tests of the new low-cost positioning algorithm developed in chapter 4 (the GCAF). The findings were that, due on the choices of auxiliary road-constraint and other low-cost GIS map-related data, as well as the estimation method designed for the purpose of optimally integrating these with GNSS, the GCAF tended to outperform existing low-cost positioning methods. This was true both in simulated environments and real-data evaluations. The design parameters of the GCAF were data driven based on the findings of chapter 3 and what was learned through the detailed analysis of positioning environments and their impacts on positioning. This data-driven improvement due to targeted design of the positioning algorithm lends support for research hypothesis 3. The key findings of chapter 5 also lent support for research hypothesis 4, based on the improvements to GCAF positioning performance due to its integration of a broader range of road-mpa related input data sources. These improvements contributed to better positioning performance when compared with existing methods. While the error reductions due to integrating traffic signal location data into positioning were non-critical, they were nonetheless observable and in all cases a non-hindrance to positioning. The use of turn restrictions/one-way street constraints in GCAF positioning presented a clear advantage over other existing methods in reducing trajectory error and the recovery times to positional fix resulting from these, thus increasing accuracy and continuity of positioning. This is in line with research trends in the literature which are looking increasingly to expanding the set of information sources from which to improve positioning performance with data integration, both in terms of low cost sources (road map data/constraints) and multi-sensor fusion approaches.

129 6.1.3 Achievement of research aims In this section the overall research purpose as stated above is disaggregated into the discrete set of tasks as deﬁned in the list of aims in chapter 1, section 1.2, and discuss their levels of achievement.

Aim 1 - Understanding positioning error Specifically, aim 1 was “To generate a more comprehensive understanding of existing positioning methods and more robustly quantify the causes and characteristics of error in GNSS-aided urban vehicle positioning”. This was associated with hypothesis 1. Overall it is concluded from the rigorous analysis of positioning error characteristics in the literature review (chapter 2), and the quantitative evaluation of performance impacts for vehicle positioning in chapter 3 that the aim has been largely achieved. This has been through the generation of original datasets that have clearly identified distinct patterns in positioning accuracy/continuity (as measured by positioning error) influenced by particular external urban environmental impacts such as urban canyons, road intersections or heavy traffic in chapters 2 and 3. Chapter 2 included a survey of the literature that contributed to this dataset by aggregating the performance evaluations of individual positioning algorithms across a range of positioning environments. Chapter 3 complemented this dataset with a set of calculations which predicted these distinct patterns as observed in chapter 2 based on simulated models from empirically validated assumptions in the literature. The chapter 3 dataset likewise found distinct impact patterns across a set of common environments which contributed to greater understanding of the nuances of positioning performance variations in an urban environment.

Aim 2 - Evaluating utility of road-constraints in positioning Aim 2 of the thesis was “to evaluate the usefulness of road constraints based on maps as a key data input for low-cost vehicle positioning, and how their usefulness is impacted by the method of road constraint integration into positioning”. Chapters 2, 3 and 5 contributed to achieving these aims. Chapters 2 and 3 thus created original datasets measuring the impacts of road constraint integration into positioning based on a new classification of road constraint integration methods (PSU and PSC, see chapter 2, section 2.2.2 for definitions). The findings were that 1) road constraints had a positive impact on positioning by removing positional ambiguity in estimation resulting from low satellite visibility or other environmental factors, 2) road constraints offered the greatest positioning performance improvement in the most hostile positioning environments and 3) positioning methods which used a PSC approach to integrating road constraints had lower positioning error and less incidence of error spikes/error growth than methods using a PSU approach.

Aims 3 and 4 - Developing and testing a new and customisable positioning method Chapters 4 and 5 of this thesis address aims 3 and 4 which are, respectively,

“To apply understanding of performance impacts of road constraints on positioning, and the causes of positioning error, to create a performance-improved (compared to existing methods) and well-tested positioning solution with a statistically optimised method for integrating road maps as a key data source for improving positioning” and

“To develop a data fusion framework that extends the scope of data sources which can be integrated into positioning (such as turn restrictions and traﬃc signal locations)”.

Chapter 4 applied existing understanding of road constraint impacts and the known causes of positioning error in low-cost positioning, as well as the new understanding generated by the investigations in chapters 2 and 3, to guide the development of an optimised low-cost positioning solution, the GCAF. Speciﬁcally, the design of the

130 GCAF targeted positioning-hostile environments where the greatest performance improvements were needed to achieve viable low-cost positioning, and also where the best performance gains could be achieved using PSC road- constraint integration methods. Chapter 5 robustly tested the GCAF in a series of evaluations, demonstrating its performance improvements over existing low-cost positioning solutions. It is therefore concluded that the outcomes from the investigations in chapters 2 and 3 were used eﬀectively to improve and optimise low-cost positioning using best-practice road-constraint and auxiliary data integration, achieving aims 3 and 4.

6.2 Further Work

This section introduces and discusses some proposals for future work in low-cost urban vehicle positioning optimisation that build on the GCAF work in this thesis. The proposals are informed by the conclusions presented in preceding sections in this chapter on the performance results of the environmental performance impact evaluation (chapter 3) and the improvements to positioning performance gained from the GCAF method compared with existing methods (chapter 5). The proposed ideas for further work use this thesis’ work to guide a more optimal methodology for achieving fully continuous and accurate (ă10m error) vehicle positioning in all urban environments. Two main ideas for further work are introduced and summarised below. These ideas are further expanded on later in this section. These proposals include:

1. Adopting an augmented multi-sensor architecture to increase the positioning dataset that is auxiliary to GNSS. This includes internal sensors such as inertial navigational systems (INS) and odometers, as well as dedicated short-range communication (DSRC) systems such as vehicle-to-infrastructure (V2I) and vehicle-to-vehicle (V2V) transceivers. The purpose of multi-sensor systems is to provide a backup input-data source to the positioning solution given GNSS is highly sensitive to urban environments and prone to becoming scarce. The backup data should act as an effective backup trajectory correction source, because despite the significant trajectory correction of optimally integrated road constraints in GNSS-hostile environments, they cannot (alone) provide adequate correction to ensure fully continuous/accurate positioning (demonstrated by the results in chapters 2, 3 and 5). 2. Developing robust methods for handling potential errors in road-maps and other GIS constraints. Errors in road-maps/GIS can include surveying errors in road co-ordinates, roads/lanes missing or incorrectly specified in the map, and trajectory constraints such as one-way/turn restrictions incorrectly assigned. Such errors may result in service-invalidating position biases and errors in the output. A statistically robust approach to accounting for potential error may correct for this incidental error source and restore service in a cost-minimal way.

The following sections expand on these ideas by providing more concrete technical discussion on how they may be implemented within the data fusion framework oﬀered by the GCAF.

6.2.1 Multi-sensor integration Example measurement model for multi-sensor ensemble

The GCAF offers a flexible data fusion framework designed to facilitate auxiliary measurement inputs zi “ pzi,1, zi,2, ..., zi,N q from (in general) N sensors. This section briefly proposes a mathematical model for iteratively approximating the posterior state probability Ppxi|Ziq based on Bayes-optimal integration of road constraints (see chapter 4) and GNSS with auxiliary sensor measurements using truncated-Gaussian approximation. Therefore auxiliary zi may be used without compromising the statistical precision of Ppxi|Ziq and position estimation of the GNSS-road map model. The measurement frequencies of the sensors may not be uniform across the ensemble, therefore some of the entries in the vector zi may be missing. The model assumes that the input data zi at time ti from N different sensors can be modeled stochastically as a jointly-nonlinear model with a stochastic component:

131 zi “ fpxi, wi; δq (6.1) where

• f : Rk`m Ñ RN maps the state vector and stochastic components to the sensor ensemble measurement output zi.

• δ is a N ˆ T matrix, where δi,j “ 1 if sensor j produces output at time ti P tt1, t2, ..., tT u.

• m is the number of dimensions of the state vector xi

N • k “ kj j“1 ř • kj is the number of stochastic variables in the measurement model stochastic vector wi,j for sensor j.

The individual sensor models are (potentially) non-linear, each with particular frequencies of output:

zi,j “ fjpxi, wi,jq (6.2) These are combined with δ to specify the joint model f a above. Examples of sensor measurement models include the nonlinear model for a single GNSS pseudorange measurement based on satellite position xS “ pxs, ys, zsq:

S 2 S 2 S 2 R ρi “ pxi ´ xiq ` pyi ´ yiq ` pzi ´ ziq ` c ∆ti ` wi (6.3) b where xi “ pxi, yi, zi, ∆tiq is the state vector at time ti including vehicle position and receiver clock bias, and wi,j is a 1-d additive stochastic noise. Examples of common models include the odometer (e.g. [7]) and inertial measurement units (IMUs) (e.g. [17]). The joint non-linear measurement model f may be equivalently described as a joint probability distribution Λpzi|xiq which need not be jointly Gaussian, discussed further in the following section.

Integrating multi-sensor measurements with road constraints

At a given sampling time ti, the GCAF data fusion will process a set of input data zi “ tzi,1, zi,2, ..., zi,N u received from the sensors as point-measurements. Since not every sensor has the same measurement frequency, some entries in this vector may be missing at a given time ti. The joint multi-sensor probability model is used to integrate the measurements optimally with road constraints and iteratively compute the posterior distribution using Bayes theorem (as per chapter 4):

Λ z x Φ x x P x Z dx X p i| iq p i| i´1q p i´1| i´1q i´1 Ppxi|Ziq “ (6.4) Λpz |x q Φpx |x q P px |Z q dx dx Xş2 i i i i´1 i´1 i´1 i´1 i In general it would be assumed that random measurement noises of each sensor are independent of the random noises of other sensors. However the independenceş assumption may be loosened if the k ˆ k covariance matrix of the stochastic sensor components is known, where the entries are given by the matrix with entries Rj1,j2 “

Covpwi,j1 , wi,j2 q. In the former case (independence), the joint probability model of the input measurements is given by a product of the probability models λjpzi,j|xiq for the individual sensor measurements (“marginal product”):

N Λpzi|xiq “ λjpzi,j|xiq (6.5) j“1 ź

132 In the latter case (dependent stochastic components between sensors with known covariance), joint probability model may be approximated as a k-variate Gaussian distribution with centre µj “ Epzi,jq and covariance matrix equal to Rj1,j2 . Following this treatment, the road constraints may be integrated using the segment-local-linearisations as described in chapter 4, section 4.2. This requires the joint probability model Λpzi|xiq to be linearised and transformed into segment-local co-ordinates. Firstly, linearisation allows the joint measurement model to be written as

zi “ Hixi ` wi (6.6) where Hi,1 Hi,2 Hi “ ¨ . ˛ (6.7) . ˚ ‹ ˚Hi,N ‹ ˚ ‹ ˝ ‚ Bzi,j and Hi,j “ is the is the measurement matrix evaluated at a linearisation point xi,0. Bxi ˇxi“xi,0 The joint measurementˇ matrix may then be co-ordinate transformed to the segment-local space, by multiplying ˇ by the Ci,j,k andˇ Di,j,k transform matrices as deﬁned in chapter 4, section 4.2. The resulting transformed measurement matrices may be used to compute the posterior covariance and local Kalman gains according to the equations in chapter 4, subsection 4.2.4. Missing measurements due to non-synchronous sensors may be handled by setting Kalman gains and corresponding point-values of zi,j to 0. In the simplest case where the sensor measurements are mutually independent linear-Gaussian models:

zi,j “ Hi,j xi ` i,j (6.8)

the joint probability model is jointly Gaussian with a k ˆ k block-diagonal covariance matrix R.

R1 0 ¨ ¨ ¨ 0 0 R2 ¨ ¨ ¨ 0 R “ ¨ . . . . ˛ (6.9) . . .. . ˚ ‹ ˚ 0 0 ¨ ¨ ¨ RN ‹ ˚ ‹ ˝ ‚ where Rj “ Covpzi,j, zi,jq If any pair of measurement noises are dependent, then the matrix 6.9 will not have block-diagonal form and th should be modeled appropriately via covariance matching, where the pi, jq entry is Σi,j.

6.2.2 Accounting for geographical errors in the road map Overview In addition to multi-sensor fusion, future work in urban vehicle positioning should involve developing models to detect and correct for the impacts of errors in road map data, including quantitative errors in the road map shape points. Road map errors can cause significant estimation biases when undetected and/or uncorrected, resulting in positioning errors and even road mis-match of the road on which the vehicle is located. Below are briefly presented some examples of how such a capability can be efficiently integrated within into the GCAF by exploiting its Bayesian locally-linear-Gaussian estimation framework.

133 Detection and correction of road position survey inaccuracy The Bayesian estimation framework of the GCAF means that parameters which are assumed to be “fixed” (known) in one model can be assumed to be unknown in a separate iteration without structural changes. For example, shape points S “ tS1,S2, ..., SM u within a road network can be set as fixed or given an error distribution πpSq which can then be efficiently inserted into the GCAF Gaussian estimation model as follows:

• µS “ EpSq

• ΣS “ CovpS, Sq then the joint-Gaussian product in equation 6.10 can be eﬃciently evaluated using standard results for Gaussian products, thus retaining the Gaussian local-structure of the posterior distribution. If the model is non-Gaussian, it may be approximated as such using a moment-match approach. The added integration over the possible adjustments to the map generates a more robust estimate that will 1) correct the original input map data and 2) lead to less biased estimation if map data is inaccurate. The cost of this added robustness is in the form of extra computational burden, however the Gaussian approximation minimises this burden using the standard properties of Gaussian products to reduce quadrature to a simple formula for Gaussian products.

134 Bibliography

[1] N. Alam, A. Kealy, and A. G. Dempster. Cooperative inertial navigation for gnss-challenged vehicular environments. IEEE Transactions on Intelligent transportation systems, 14(3):1370–1379, 2013. [2] M. Arulampalam, M. S. Maskell, N. Gordon, and T. Neil. A tutorial on particle ﬁlters for on-line non- linear/non-Gaussian Bayesian tracking. IEEE Transactions on Signal Processing, 50:174188, 2002.

[3] A. Asadian, B. Moshiri, and J. Rezaie. Robustness assessment of intelligent information fusion techniques in navigation systems encountering satellite outage. In Innovations in Information Technology, 2007. IIT’07. 4th International Conference on. IEEE. [4] A. Asadian, B. Moshiri, and J. Rezaie. Robustness assessment of intelligent information fusion techniques in navigation systems encountering satellite outage. In Innovations in Information Technology, 2007. IIT’07. 4th International Conference on. IEEE, 2007, pages 46–50, 2007. [5] U. I. Bhatti and W. Y. Ochieng. Failure modes and models for integrated gps/ins systems. Journal of navigation, 60(2):327–348, 2007. [6] H. A. P. Blom and Y. Bar-Shalom. The Interacting Multiple Model Algorithm for systems with Markovian switching coefficients. IEEE Transactions on Automatic Control, 33(8):780–783, 1988. [7] C. Boucher and J. Noyer. A hybrid particle approach for GNSS applications with partial GPS outages. IEEE Transactions on Instrumentation and Measurement, 59(3):498–505, 2010. [8] J. Chao, Y. Q. Chen, W. Chen, X. Ding, Z. Li, N. Wong, and M. Yu. An experimental investigation into the performance of gps-based vehicle positioning in very dense urban areas. Journal of Geospatial Engineering, 3(1):59–66, 2001. [9] G. Chen and M. Harigae. Using imm adaptive estimator in gps positioning. In SICE 2001, pages 78–83, 2001. [10] W. Chen, M. Yu, Z. L. Li, and Y. Q. Chen. Integrated vehicle navigation system for urban applications. In Proceedings of the 7th International Conference on Global Navigation Satellite Systems (GNSS), European Space Agency, Graz, Austria, pages 15–22, 2003. [11] Y. Cheng and T. Singh. Efficient particle filtering for road-constrained target tracking. IEEE Transactions on Aerospace and Electronic Systems, 43(4):1454–1469, 2007. [12] N. Cui, L. Hong, and J. R. Layne. A comparison of nonlinear filtering approaches with an application to ground target tracking. Signal Processing, 85(8):1469–1492, 2005.

[13] Y. Cui and S. Ge. Autonomous vehicle positioning with gps in urban canyon environments. Robotics and Automation, IEEE Transactions on, 19(1):15–25, 2003. [14] F. Daum. Nonlinear ﬁlters: beyond the kalman ﬁlter. Journal of navigation, 58(2):273–282, 2005.

135 [15] F. Daum. Nonlinear ﬁlters: beyond the kalman ﬁlter. Aerospace and Electronic Systems Magazine, IEEE, 20(8):57–69, 2005.

[16] R. Douc and O. Cappe. Comparison of resampling schemes for particle filtering. In Image and Signal Processing and Analysis, 2005. ISPA 2005. Proceedings of the 4th International Symposium on. IEEE, 2005. [17] M. El-Diasty and S. Pagiatakis. An efficient INS/GPS impulse response model for bridging GPS outages. In IEEE TIC-STH 2009, pages 328–333, 2009. [18] EU-Commission. Directive on the framework for the deployment of intelligent transport systems in the field of road transport and for interfaces with other modes of transport. Official Journal of the European Union, 40:1–13, 2010. [19] N. Faouzi, H. Leung, and A. Kurian. Data fusion in intelligent transportation systems: Progress and challengesa survey. Information Fusion, 12(1):4–10, 2011.

[20] J. Georgy. Nonlinear filtering for tightly coupled riss/gps integration. In Position Location and Navigation Symposium (PLANS), 2010 IEEE/ION, pages 1014–1021, 2010. [21] N. J. Gordon, D. J. Salmond, and A. F. M. Smith. Novel approach to nonlinear/non-Gaussian Bayesian state estimation. In IEE Proceedings F (Radar and Signal Processing), volume 140, pages 651–654, 1993. [22] F. Gustafsson, F. Gunnarsson, N. Forssell, U.Forssell, J. Jansson, R. Karlsson, and P. Nordlund. Particle filters for positioning, navigation, and tracking. IEEE Transactions on Signal Processing, 50(2):425–437, 2002. [23] F. Gustafsson, U. Orguner, T. B. Schon, P. Skoglar, and R. Karlsson. Handbook of Intelligent Vehicles, chapter 16, pages 399–433. Springer, 2012. [24] A. Hata and D. Wolf. Road marking detection using lidar reective intensity data and its application to vehicle localization. In IEEE 17th International Conference on Intelligent Transportation Systems (ITSC), pages 584–589, 2014. [25] B. Hoffmann-Wellenhof, H. Lichtenegger, and J. Collins. GPS Theory and Practice. [26] S. J. Julier and H. F. Durrant-Whyte. On the role of process models in autonomous land vehicle navigation systems. Robotics and Automation, IEEE Transactions on, 19(1):1–14, 2003.

[27] R. E. Kalman. A new approach to linear ﬁltering and prediction problems. Transactions of the ASME–Journal of Basic Engineering, 82(Series D):35–45, 1960. [28] W. Kim, G. Jee, and J. Lee. Eﬃcient use of digital road map in various positioning for its. In Position Location and Navigation Symposium, IEEE, pages 170–176, 2000.

[29] T. Kirubarajan and Y. Bar-Shalom. Kalman Filter versus IMM estimator: When do we need the latter? IEEE Transactions on Aerospace and Electronic Systems, 39(4):1452–1457, 2003. [30] T. Kirubarajan, Y. Bar-Shalom, K. R. Pattipati, and I. Kadar. Ground target tracking with variable structure IMM Estimator. IEEE Transactions on Aerospace and Electronic Systems, 36(1):26–46, 2000.

[31] J. Koller and M. Ulmke. Data fusion for ground moving target tracking. In IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems, pages 217–224, 2006. [32] J. H. Kotecha and P. M. Djuric. Gaussian particle ﬁltering. Signal Processing, IEEE Transactions on, 51(10):2592–2601, 2003.

136 [33] E. J. Krakiwsky, C. B. Harris, and R. V. C. Wong. A kalman ﬁlter for integrating dead reckoning, map matching and gps positioning. In Position Location and Navigation Symposium, 1988. Record. Navigation into the 21st Century. IEEE PLANS’88., IEEE, pages 39–46, 1988.

[34] X. Lin, T. Kirubarajan, Y. Bar-Shalom, and X. Li. Enhanced accuracy GPS navigation using the interacting multiple model estimator. In Aerospace Conference, 2001, IEEE Proceedings. Vol. 4. IEEE, 2001, 2001. [35] L. J. Lyrio, T. Oliveira-Santos, C. Badue, and A. Ferreira De Souza. Image-based mapping, global localization and position tracking using vg-ram weightless neural networks. In 2015 IEEE International Conference on Robotics and Automation (ICRA), pages 3603–3610, 2015. [36] P. Nordlund and F. Gustafsson. Sequential Monte Carlo filtering techniques applied to integrated navigation systems. In Proceedings of the American Control Conference Arlington, VA June 25-27, 2001, pages 4375–4380, 2001. [37] W. Y. Ochieng, M. Quddus, and R. B. Noland. Map-matching in complex urban road networks. Revista Brasileira de Cartografia, 2(55), 2009. [38] Andrej Peisker, Mark Richard Morelande, and Allison Kealy. A new non-linear filtering algorithm for road- constrained vehicle tracking. In 2014 Ubiquitous Positioning Indoor Navigation and Location Based Service, UPINLBS 2014, Corpus Christ, TX, USA, November 20-21, 2014, pages 54–63, 2014.

[39] B. P. Phuyal. Method and use of aggregated dead reckoning sensor and gps data for map matching. In Proceedings of the 15th International Technical Meeting of the Satellite Division of The Institute of Navigation (ION GPS 2002), pages 430–437, 2001. [40] O. Pink. Visual map matching and localization using a global feature map. In 2008 IEEE Computer Society Conference on Computer Vision and Pattern Recognition Workshops, pages 1–7, 2008.

[41] M. K. Pitt and N. Shephard. Filtering via simulation: Auxiliary particle ﬁlters. Journal of the American statistical association, 94(446):590–599, 1999. [42] M. A. Quddus, W. Y. Ochieng, and R. B. Noland. Current map-matching algorithms for transport applications: State-of-the art and future research directions. Transportation Research Part C: Emerging Technologies, page 312328, 2007.

[43] 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, 2003. [44] S. F. Schmidt. Application of state-space methods to navigation problems. Advances in Control Systems, 3:293340, 1966.

[45] D. Simon. Kalman ﬁltering with state constraints: a survey of linear and nonlinear algorithms. IET Control Theory and Applications, 4(8):1303–1318, 2010. [46] I. Skog and P. Hndel. In-car positioning and navigation technologiesa survey. IEEE Transactions on Intelligent Transportation Systems, 10(1):4–21, 2009.

[47] Morgan Stanley. Directive on the framework for the deployment of intelligent transport systems in the ﬁeld of road transport and for interfaces with other modes of transport. Oﬃcial Journal of the European Union, 40:1–13, 2010. [48] D. Streller. Road map assisted ground target tracking. In 11th International Conference on Information Fusion, pages 80–92, 2008.

137 [49] N. Suganuma and T. Uozumi. Precise position estimation of autonomous vehicle based on map-matching. In 2011 IEEE Intelligent Vehicles Symposium (IV), pages 296–301, 2011.

[50] S. Taghipour, M. Reza Meybodi, and A. Taghipour. An algorithm for map matching for car navigation system. In 3rd International Conference on Information and Communication Technologies: From Theory to Applications, 2008. ICTTA 2008, pages 1–5, 2008. [51] G. Taylor, G. Blewitt, D. Steup, S. Corbett, and A. Car. Road reduction ﬁltering for gpsgis navigation. Transportation Research Part C, 5(3):193–207, 2001.

[52] P. Tichavsky, C. H. Muravchik, and A. Nehorai. Posterior cramer-rao bounds for discrete-time nonlinear ﬁltering. IEEE Transactions on Signal Processing, 46(5):1386–1396, 1998. [53] E. Tzoreﬀ and B. Bobrovsky. A novel approach for modeling land vehicle kinematics to improve GPS performance under urban environment conditions. IEEE Transactions on Intelligent Transportation Systems, 13(1):344–353, 2012.

[54] M. Ulmke and W. Koch. Road-map assisted ground moving target tracking. IEEE Transactions on Aerospace and Electronic Systems, 42(4):1264–1274, 2006. [55] R. Vivacqua, R. Vassallo, and F. Martins. A low cost sensors approach for accurate vehicle localization and autonomous driving application. Sensors, 17(10):2359, 2017.

[56] C. E. White, D. Bernstein, and A. L. Kornhauser. Some map matching algorithms for personal navigation assistants. Transportation Research Part C, 8:91–108, 2000. [57] M. Yu, Z. Li, Y. Chen, and C. Wu. Improving integrity and reliability of map matching techniques. Information Fusion, 5(1):40–46, 2011.

[58] J. Zhang, F. Y. Wang, K. Wang, W. H. Lin, X. Xu, and C. Chen. Data-driven intelligent transportation systems: A survey. Intelligent Transportation Systems, IEEE Transactions on, 12(4):1624–1639, 2011. [59] X. Zhang, Q. Wang, and D. Wan. Map matching in road crossings of urban canyons based on road traverses and linear heading-change model. Instrumentation and Measurement, IEEE Transactions on, 56(6):2795–2803, 2007.

[60] Y. Zhao, B. Song, and J. Li. A map matching algorithm in gps-based car navigation system. In Intelligent Information Hiding and Multimedia Signal Processing, 2007. IIHMSP 2007. Third International Conference on, volume 1, pages 77–80, 2007.

138 Appendices

139 Appendix A

Proof of Lemma 1

A.1 Nonlinear road constraint integration with GNSS: analytical challenges A.2 Background to this analysis

To determine an optimal method of integrating road constraints with the stochastic models as discussed in section 4.1.4, it is important to consider the associated computational complexity issues. This Appendix evaluates the practical real-time-processing feasibility of recursive-Bayesian Gaussian Cluster positioning with hard constraints. Lemma 1 is the result that the exact solution to the Bayes positioning equation (see equation 2.1), given simple model assumptions about GNSS measurements and dynamics, has exponentially growing computational complexity with time when constrained by the geometry of a road map. The corollary from Lemma 1 is therefore that the exact solution is not a feasible approach to low-cost road-constrained positioning due to the non-viability of real-time output. A proof of Lemma 1 is provided in this Appendix which concludes therefore that an approximate solution to equation is necessary to achieve viable but statistically rigorous low-cost positioning. Where notation or variables are not explicitly deﬁned in this Appendix, please refer to chapter 4 for the original deﬁnitions.

A.3 Road segment boundary eﬀects

Let us consider an arbitrary road network N composed of segments 1, ..., NS connected at junctions and which is non-trivial and connected. N is a set containing all physical locations x P N within the road network. Non-trivial means that N contains junctions with more than two segments radiating therefrom, and connected means that a continuous path exists between any two points px1, x2q P N such that the path lies entirely in N. An initial estimate of the vehicle position at time t0 is given by a Gaussian cluster as discussed in section 4.1.3, subsection “Initialisation of the Gaussian cluster Pˆpx0q”:

n0 j Ppx0q “ w0 N px0; µ0,j, P0,j,I0,j,Sjq (A.1) j“1 ÿ where

• x0 is the state at time t0, with components as deﬁned in section 4.1.4, subsection “State Variables”

• n0 is the chosen number of components in the initial cluster

140 j th ˆ • w0 is the weight of the j component of Ppx0q

th • I0,j is the initial dynamical state index assigned to the j component of Pˆpx0q

th • Sj is the integer label of the segment containing the j component of Pˆpx0q The constraints are made to be state-restricting, meaning the positional domain of the vehicle is restricted to xi P N This results in truncation of the cluster components at segment boundaries, with the probability mass forced onto neighbouring segments (see ﬁgures 4.2 and 4.5 for visualisation). Let us also assume that the linear-Gaussian models for the state-evolution is as speciﬁed for the GCAF (equations 4.5 with identical parameters), and that the GNSS measurements are of SPP type, modeled by equation 4.9 with identical parameters.

p1,1q If P0,j ą 0 @j is assumed (initial cluster has does not include delta distribution “particles”) then the probability mass of Pˆpx0q necessarily covers multiple segments (given the non-trivial network assumption). The marginal prior ˆx distribution over position P0 “ V,H,T Ppx0; µ0, P0q dx dv d∆t is therefore equivalent to a sum of truncated Gaussian pieces which are clipped at segment boundaries due to the constraints, but align at the boundaries: ş N j T ||x0 ´ µ0|| Ppx0q “ wj φ ; Sj (A.2) j“1 ˆ pP0,jqp1,1q ˙ ÿ where: b φpxq 1. φT px; Sq “ 1S is the probability density function of a truncated Gaussian (with boundaries deﬁned S φpxq dx by the extent of set S) and φpxq is the probability density function of a N px; 0, 1q distribution ş j ˆ 2. µ0 is the positional mean of Ppx0q when oriented along segment Sj (which may lie outside the physical bounds of Sj), and x0 is the initialised vehicle position

th 3. Sj Ă X is the truncated support of the truncated Gaussian corresponding to the j segment

j ||x ´ µ0|| 4. wj “ φ dx are normalising weights to ensure that Sj σ ˆ x,0 ˙ P0 dx “ 1 X ş

The propagation of Pˆpx0q via the evolution model (equation 4.5) will necessarily propagate and preserve the truncated-Gaussianş structure, although the truncated Gaussian weights and parameters (means and covariances) are adjusted according to the propagation equations (see 4.5). Intuitively, the probability mass ﬂows continuously across segment boundaries (irrespective of trajectory changes) and does not encounter structural distortion because the Gaussian pieces are aligned across segment boundaries, even when splitting occurs. Alignment of two Gaussian components at a boundary point xB means that their normalised slopes

1 B ||x ´ µj || φT 0 (A.3) wj B x p1,1q ˆ P ˙ˇx“xB 0,j ˇ ˇ are equal. b ˇ If a GNSS measurement is available at t1, the updated-Pˆpx1|Z1q will be computed based on the likelihood mass redistribution described in equation 2.1. Although Pˆpx1|Z1q is still a truncated Gaussian sum (with GNSS- updated weights and parameters), the smooth alignment across segment boundaries is lost if the segments have

141 diﬀerent heading directions (see ﬁgure A.1). This is because the measurement update can be considered a sum of inhomogeneous locally-linear updates on the separate segments caused by the nature of the network geometry and the projection of circular Gaussian measurement likelihood distribution

x y z1 ´ x1 z1 ´ y1 Λpz1|x1q “ φp q φp q (A.4) σz σz onto the locally-linear network. The symbols in equation A.4 are as defined in section 4.1.4, subsection “Measure- ment model for SPP-type GNSS measurements”. Equivalently, the segment-specific Hi matrices are parameter-sensitive to the orientations of the segments and do not match at the segment edges. An intuitive visualisation is that slicing a bivariate Gaussian in an “V” shape results in a non-smooth kink across the corner of the slice (see figures A.2 and A.1), which is analogous to the projection of the circularly-symmetric SPP Gaussian measurement error onto a “V” shaped road constraint.

Figure A.1: Visual representation of the process of Gaussian alignment loss across a boundary point due to integrating GNSS measurements.

142 Figure A.2: Visual representation of alignment loss across boundary points due to SPP-type GNSS measurements. An SPP point-measurement is modeled as an elliptical Gaussian with contour region shown, superimposed on a road corner (top). When projected onto the “L” shaped road, the Gaussian probability has two peaks and a non-smooth kink at the boundary point along the road, with no mass on oﬀ-road areas (bottom).

Since the domain of the components of Pˆpx1|Z1q is unbounded, and the network is connected, this implies that such a case of alignment-loss will necessarily occur somewhere within its structure following the ﬁrst measurement update. A Gaussian which has undergone alignment-loss is referred to as “non-aligned”. As demonstrated in the next section, a non-aligned truncated Gaussian posterior Pˆpx1|Z1q will necessarily not retain Gaussian structure following a subsequent linear-Gaussian propagation under Φpx2|x1q at time t2 and beyond.

A.4 Loss of Truncated Gaussian Structure following linear propagation

Let us consider a multivariate truncated Gaussian component TN i,j with probability density function (pdf) 1 T Sj ˆV ˆHˆT αj 1 ´1 φ pxi; Sjq “ exp ´ pxi ´ µiqΣ pxi ´ µiq on segment Sj at time ti, where αj is a nor- 4π2 Σ 2 i | i| ˆ ˙ malising constant and pµi, Σiq are its centre/scale parameters. Let us assume that TN i,j is propagated linearly along a section of road.a The linear propagation model is

xi`1 “ Fi xi ` wi (A.5) where

1 δti 0 0 0 1 0 0 F “ i ¨0 0 1 0˛ ˚0 0 0 c‹ ˚ ‹ ˝ ‚ , c is the speed of light, and wi is a Gaussian process noise with 0 mean and covariance

143 0 0 0 0 2 0 σv 0 0 Qi “ ¨ 2 ˛ 0 0 σh 0 ˚0 0 0 σ2 ‹ ˚ ∆t‹ ˝ ‚ The pdf of TN i,j following the propagation can then be computed as an integral

T φ pxi`1q “

v ´ v h ´ h ∆t ´ ∆t φ i`1 i φ i`1 i φ i`1 i δpx ´ x ´ v δtq σ σ σ i`1 i i ˆ v ˙ ˆ h ˙ ˆ ∆t ˙ żSj ,V,H,T T ˆ φ pxi; Sjq dxi dvi dhi d∆ti

h ´ h ∆t ´ ∆t “ φ i`1 i φ i`1 i Ipx , v |h , ∆t q dh d∆t (A.6) σ σ i`1 i`1 i i i i ˆ h ˙ ˆ ∆t ˙ żH,T where

Ipxi`1, vi`1|hi, ∆tiq “

vi`1 ´ vi T φ δpxi`1 ´ xi ´ vi δtq φ pxi; Sjq dxi dvi dvi σv Sj ,V ˆ ˙ ż xi`1 ´ aj δt v ´ v “ φ i`1 i φT px ´ v δt, v , h , ∆t ; S q dv (A.7) σ i`1 i i i i j i xi`1 ´ bj ˆ v ˙ ż δt T The Ipxi`1, vi`1|hi, ∆tiq, and therefore φ pxi`1q, is not Gaussian in xi`1 because of the presence of the state variable in the integral bounds. This is caused by truncated-Gaussian shape which is not self-similar under linear- Gaussian propagation, unlike a pure (non-truncated) Gaussian component. This is referred to as the truncation- T propagation effect or TPE. The analytical form of the propagated component solution φ pxi`1q is only expressible as non-Gaussian differences of multivariate error functions (which is a significant distortion to the original Gaussian shape).

This result extends to all truncated components that are propagated using the same dynamical state-evolution model in a network if those segments are non-aligned at common boundary points following a GNSS update. If they are aligned, then the TPE is mathematically offset by the neighbouring segment, and the distortions exactly cancel out. This ceases if the alignment is broken; the combined effect is an analytically non-reducible sum of error (“erf”) functions. Lemma 1 states therefore that any Gaussian cluster posterior Pˆpxi|Ziq in a segmented road network at time ti will, if the first GNSS measurements are integrated at time ti, cease to be a Gaussian cluster at time ti`1 and thereafter.

The computational complexity for times ti`1 and above become exponentially greater as no simple closed-form solutions for the moments of nested error function integrals exist at these higher iterations, and must be computed

144 through nested quadrature. This results in exponentially growing computational complexity due to linearly growing number of nested dimensions, therefore the exact solution to the propagated/updated posterior cannot be computed in real-time.

A.5 Extension of Lemma 1 to pseudorange measurements and GNSS- deprived periods

The proof of Lemma 1 was specific to SPP-type GNSS measurements, however the same reasoning can be applied to the case of code-based pseudorange measures, assuming a standard linearised model of the range equation (see equation 4.8). Due to the orientation sensitivity of the pseudorange model (similar to that of the SPP), the posterior non-alignment across segment boundaries also appears in the case of pseudorange measures. This is because, like the SPP case, the H matrix projection along each segment (used in the segment-wise update) is sensitive to the orientation of the segment due to the projection of the Gaussian likelihood. Therefore the GNSS-updated posterior across the boundaries of heading-disparate segments will not align, resulting in the TPE becoming realised and computational complexity growing exponentially in future iterations. If GNSS is not available at time t1, then the aligned truncated Gaussian posterior may be propagated until the first GNSS measurement is obtained at time tk, k ą 1, at which time the non-alignment is first introduced at time tk. Since propagation of aligned pieces is computationally simple, and alignment is preserved in the absence of GNSS measurement integration, an exact, real-time solution to equation 2.1 is possible between t0 and tk. However this ceases following the integration of GNSS measurements and cannot be restored.

A.6 Conclusion

The results of Lemma 1 lead to the conclusion that a linear filter such as the Kalman Filter can never be integrated with road constraints if a statistically exact AND real-time positioning solution is sought. This implies that an advanced non-linear filtering solution is invariably required to achieve such an aim. The Lemma 1 proof has identified the precise mathematical causes of the analytical complexity (GNSS integration and Gaussian non-alignment), and that the root cause of the complexity growth cannot be removed because it is inherent to the typical assumptions of GNSS-aided positioning. An effective positioning solution must therefore combat the complexity growth problem by matching growth with artificial complexity-reductions. These conclusions have important analytical implications for and guide the development of the GCAF iterative protocol (see chapter 4 section 4.2.5), which combines the road constraints with the dynamical and measurement models discussed in section 4.1.4 to achieve real-time low-cost and statistically robust positioning.

145 Appendix B

Calculation of GCAF measurement update weights γi,j,k in equation 4.19

This Appendix contains calculations of the GCAF measurement weights γi,j,k for the Gaussian components on each road network segment as required to generate the position estimate via equation 4.19 when using pseudorange measurements (section B.1 or SPP (section B.2). Where notation or variables are not explicitly deﬁned in this Appendix, please refer to chapter 4 for the original deﬁnitions.

B.1 Calculating γi,j,k when using pseudoranges The weights can be written in terms of the contributions from each pseudorange measurement (of which there are Ki at time ti) multiplied by the initial probability weight on the segment:

pKiq γi,j,k “ κi,j,k τi,j,k (B.1) where

x ´ pî,j,k • τi,j,k “ φ dx Si,j,k σˆ ˆ i,j,k ˙ • pî,j,k isş the p-component of xî,j,k 2 ˆ p1,1q • σî,j,k “ li,j,k Pi,j,k

pKiq • κi,j,k is computed via the parallel recursions described in equations B.2, iterated over the satellite indices m “ 1, ..., Ki at time ti

p0q p0q Assuming κi,j,k “ 1 and xi,j,k “ xˆi,j,k, the parallel weight recursions are given by:

pmq pmq pm´1q 1 ´θi,j,k κi,j,k “ κi,j,k expt u (B.2) pmq 2pm´1q 2pσpm´1qq ri ` σi,j,k i,j,k b where σ2 • rpmq ρ i,j,k “ pmq sinpφi,j,kq

146 pmq • φi,j,k is the angle of elevation from point Mppˆi,j,kq

2pmq 2 pmq p1,1q • σi,j,k “ li,j,k Σi,j,k

T pmq pmq pmq pmq • Σi,j,k “ Ai,j,k Pi,j,k Ai,j,k ´ ¯ pmq pmq pmq pm´1q • Pi,j,k “ pI ´ Ki,j,k Hi,j,kq Pi,j,k p0q ˆ • Pi,j,k “ Pi,j,k

pmq th • Hi,j,k is the m row of Hi,j,k

pm,1q pm,2q pm,3q pm,4q Hi,j,k Hi,j,k Hi,j,k Hi,j,k pmq 0 1 0 0 • A “ ¨ ˛ i,j,k 0 0 1 0 ˚ 0 0 0 1 ‹ ˚ ‹ ˝ ‚ T T pmq pm´1q pmq pmq pm´1q pmq pmq • Ki,j,k “ Pi,j,k Hi,j,k Hi,j,k Pi,j,k Hi,j,k ` rj ´ ¯ ˆ´ ¯ ˙ pmq pmq pmq pm´1q • θi,j,k “ ρi ´ Hi,j,k xi,j,k

pmq pm´1q pmq pmq • xi,j,k “ xi,j,k ` Ki,j,k θi,j,k

B.2 Calculating γi,j,k when using single point positions (SPP) If the measurements are SPP measurements, then the weights are given by:

ξi,j,k γi,j,k “ (B.3) n Nj,k ξi,j,k k“1 i“1 ř ř where

• ξi,j,k “ τi,j,k ∆Zi,j,k ∆Xi,j,k

K |zi ´ z | • ∆Z “ φ i,j,k i,j,k σ ˆ z ˙ K |Hi,j,k ˆxi,j,k ´ zi,j,k| • ∆Xi,j,k “ φ 2 2 ˆ σˆi,j,k ` σz ˙ b • | ‹ | denotes vector magnitude

K • zi,j,k is the perpendicular projection of zj onto segment Si,j,k, see ﬁgure B.1

147 K Figure B.1: Perpendicular projections zi,j,k of a geographically mapped SPP measurement zj onto the linear extensions of segments Si,j,k.

148

Minerva Access is the Institutional Repository of The University of Melbourne

Author/s: Peisker, Andrej

Title: Optimising low-cost GNSS positioning for road constrained vehicles using a Bayesian filter

Date: 2018

Persistent Link: http://hdl.handle.net/11343/219254

File Description: Complete (corrected) thesis

Terms and Conditions: Terms and Conditions: Copyright in works deposited in Minerva Access is retained by the copyright owner. The work may not be altered without permission from the copyright owner. Readers may only download, print and save electronic copies of whole works for their own personal non-commercial use. Any use that exceeds these limits requires permission from the copyright owner. Attribution is essential when quoting or paraphrasing from these works.