Map-constrained Adaptive GNSS/IMU Fusion Scheme for Robust Urban Navigation

Transcription

1 Map-constrained Adaptive GNSS/IMU Fusion Scheme for Robust Urban Navigation Mohamed M. Atia and Steven. L. Waslander Abstract Recent advances in Global Navigation Satellite Systems (GNSS) and integration with high-rate sensors greatly enhance pose estimation performance in harsh environments. However, urban areas often lead to position errors due to multipath signals. Adaptive sensor fusion methods have been shown to handle GNSS outliers by adaptively estimating measurement noise covariance. However, existing methods depend on innovation sequences and, consequently, they fail if biased errors exist or insufficient redundant measurements are available. This paper proposes a map-aided adaptive fusion algorithm that uses map constraints to adaptively estimate GNSS position measurement noise covariance. The method estimates the currently active map segment using dead-reckoning and a robust map-matching algorithm that considers the vehicle state history, roads geometry, and map topology in a Hidden-Markov Model (HMM), and the Viterbi algorithm is used to decode the HMM model and select the most likely segment. By identifying the most likely map segment, the projection of vehicle states onto the map segment is used as a supplementary position update to the integration filter. GNSS covariance is then modeled as a subcovariance matrix of a multivariate Gaussian distribution which is estimated conditioned on map-constraints using Bayesian inference. The proposed solution framework has been developed in ANSI C and tested on a land-based vehicular platform in downtown Toronto using a 3 Space inertial measurement unit, a Ublox EVK-7 GNSS receiver kit, and digital maps from the province of Ontario. Results showed accurate map segment estimation in difficult roads intersections, forks, and joins. The adaptive GNSS fusion scheme proved to reliably mitigate biased GNSS position updates that are falsely reported as accurate by the GNSS receiver. Index Terms Sensor Fusion, Map Matching, Hidden Markov Models, Adaptive Extended Kalman Filter, Integrity Monitoring N I. INTRODUCTION ext-generation intelligent vehicles will require highly precise and robust positioning systems to fulfill the demands of a wide range of applications. The self-driving car is an example of an emerging technology where accuracy and integrity are critical requirements for safe guidance and stable control. Current vehicular positioning systems are dominated by Global Navigation Satellite Systems (GNSS) such as the Global Positioning System (GPS) []. According to the FAA GPS Performance Analysis Report [], horizontal accuracy of GPS (Standard Positioning Service SPS ) is within 3.35m with a 95% confidence level. However, in urban canyons, accuracy is affected by sky blockage and receiver quality. GNSS accuracy can be significantly improved using several techniques such as differential GNSS (DGNSS), Augmented GNSS, and Precise Positioning Services (PPS) [] [3] [4]. However, these techniques add complexity, and additional cost. Multi-constellation GNSS [5] also enhances the accuracy by increasing number of visible satellites in the sky. However, in dense urban areas where sky occlusion due to high buildings is common, the geometry of visible satellites often results into high uncertainty in the vehicle s GNSS position estimate [6]. Therefore, and despite the aforementioned advances in GNSS technology, performance in dense urban areas is still challenging. To further demonstrate the problem, Figure shows the performance of a standard SPS GPS receiver in an urban area in downtown Toronto where the effect of multipath is significant. Recent advanced receivers apply internal filtering, motion models [7], utilize multi-constellation satellites, and/or accept corrections from GNSS augmentation satellites (e.g SBAS, WAAS). Therefore, performance of such receivers is greatly improved. However, under multipath conditions, these advanced GNSS receivers sometimes introduce multiple outliers that may look consistent. Figure and Figure 3 show the performance of an SBAS-enabled GPS/ GLONASS receiver in downtown Toronto. The yellow boxed areas in Figure and Figure 3 clearly show biased positioning errors. The problem becomes more complex when the receiver reports small Dilution of Precision (DOP) with these erroneous positioning measurements. Although GNSS receivers perform internal receiver autonomous integrity monitoring (RAIM) [4] [3] and provide accuracy measures such as DOP [3], experiments in urban areas [6] show that the estimated DOP is not always consistent with true accuracy. This can be seen in Figure 4 where the horizontal DOP (HDOP) reported by the GNSS receiver is scaled and used to draw a circle around the estimated locations. The yellow-circled locations in Figure 4 show erroneous positioning while the GNSS receiver reports an accuracy similar to other points where positioning is accurate. Figure 4 also shows that this type of error is not zero-mean random noise as is the case in SPS GPS performance shown in Figure. A common approach to enhance overall accuracy is the integration with other sensors such as inertial measurement units (IMU) [3], laser scanners [8], and vision sensors [9]. Another aiding source that can be used is digital maps []. M. M. Atia is with the department of mechanical and mechatronics at university of Waterloo, Ontario, Canada ( mohamed.maher.atia@ gmail.com). S. L. Waslander is with the department of mechanical and mechatronics at university of Waterloo, Ontario, Canada ( stevenw@uwaterloo.ca).

2 However, in integrated systems, the positioning performance ultimately depends on the accuracy of GNSS measurements. An erroneous GNSS position update will either degrade the accuracy or even cause the filter to diverge if not properly weighted or rejected. Any inconsistency in GNSS estimated accuracy makes it difficult for integration filters to reject or properly weight these biased errors. For critical applications such as self-driving cars or rescue missions in urban areas, such errors are not acceptable. Therefore, there is a need for a robust integration scheme that mitigates the effects of these kinds of errors. Figure 4. The figure displays the inconsistent accuracy estimated by GNSS in an urban area Figure. Example of SPS GPS performance in an urban area in downtown Toronto. Position outliers dominate the solution. Figure. Example of augmented GNSS biased position error in Downtown Toronto. Figure 3. Example of augmented GNSS biased position error in Downtown Toronto Figure 5.Illustrative figure for the map-constrained adaptive GNSS position accuracy estimation. Blue is GNSS with biased errors. Red is map-matched sensor fusion solution. This paper introduces a robust map-constrained adaptive fusion framework that overcomes the aforementioned problems. The paper presents three basic improvements. The first contribution is the development of an enhanced mapmatching framework that uses a variable-size Markov chain and a Hidden-Markov Model (HMM) [] algorithm that accurately matches vehicle s pose (position and orientation) history with road geometry and map topology. The second contribution is the development of a map-constrained adaptive GNSS noise covariance estimation technique using Bayesian inference that is more robust against both zero-mean random noise and biased GNSS errors and, at the same time, enhances the convergence of the integration filter by taking feedback from map-matched positions. The proposed map-aided adaptive fusion method estimates consistent GNSS position error covariance as illustrated in Figure 5. Note that the error ellipses reflect low uncertainty when GNSS is consistent with the true position. It also reflects higher uncertainty in the direction of the road map segment. The third contribution is that, in contrast to many common existing adaptive Kalman filter methods [] [3] [4] [5], the proposed adaptive framework does not depend on redundant pseudo-ranges, residuals or the innovation sequence, which make it suitable for use with arbitrary noise characteristics and varied integration schemes (e.g looselycoupled or tightly-coupled).

3 II. PROBLEM FORMULATION The navigation problem can be modelled as a dynamic system of states as follows: xt () x( t) f ( x( t), u( t)) w(t) () y( t) h( x( t)) v(t) () where f (.) system noise, measurements, v(t) is a nonlinear dynamic model, w(t) is a stochastic u(t) is a control signal, yt () is external is a nonlinear measurement model and h(.) is a stochastic measurement noise. By linearizing () and () we have: x( t) F( t) x( t) G( t) w( t) (3) y( t) H ( t) x v( t) (4) f ( x( t), u( t)) (5) Ft () x h( x( t)) (6) Ht () x The Kalman Filter provides optimal estimation of assuming and v( ) are zero-mean Gaussian noise with covariance: wt () Q( t) w( t) w( t) T R( t) v( t)v( t) T where P: x t x (7) (8) is Gaussian with zero mean and a covariance matrix P( t) x( t) x( t) T (9) Equations (3) and (4) can be written in discrete form with sample T: () xk ( I FkT ) xk wk y H x v () k k k k The optimal estimation of the error vector, measurements, xk, given y k, is calculated using two steps: prediction, x () k f ( xk, uk ) P ( I F T) P ( I F T) T Q (3) k k k k k and update, K P P R T T k k H k(hk k H k k) (4) k k k k k x x K y h( x ) (5) P ( I K H ) P (6) k k k k Equations (4-6) show that the correction gain (K) and state error covariance (P) depend on Q and R. To obtain the best performance, the Q and R matrices need to be carefully chosen. The common approach is to estimate Q from some sensors specifications, manufacturer settings or stochastic modelling (i.e. Allan Variance) and try to adapt R to obtain the best performance. Manual tuning by experts for a specific system is also a common approach. However, having R fixed makes the filter vulnerable to divergence and errors if the environment changes. The objective of this work is to adaptively estimate R for a ground-vehicle navigation system that fuses measurements from IMU, GNSS, and maps. III. RELATED WORK Several methods have been proposed to address the problem of adaptively estimating the noise covariance matrix for best filter performance. An early approach is presented in [5], where a history of the error vector is used to estimate the Q matrix and the scaled DOPs calculated by GNSS receivers are used to adapt the R matrix. The problem with these methods is the dependency on the receiver s calculated DOPs, which are not always consistent with the true accuracy, as is visible in Figure 4 and Figure 5. Another method that depends on post-processing of trajectory data under different Q and R values was proposed in [4]. However, the proposed method is completely manual and it does not consider dynamic environment changes. In [6], image feature measurements are adaptively weighted heuristically based on image intensity and IMU measurements. The weighted measurements are then used by an adaptive least-square algorithm to estimate visual odometry of a robotic platform with a stereo vision system. A similar method is applied to the Kalman filtering in [], where the filter innovation sequence vector, y h( x ) k k x, is used to adaptively adjust the R matrix. An interesting work that applies dynamic motion constraints in addition to the innovation sequence to adaptively weight measurements has been proposed in [3]. In this work, the motion of the moving platform is predicted from the dynamic history over a short period of time. The constrained state prediction is performed by a Newton forward difference extrapolation technique. External measurements are then corrected by comparing external measurements to the predicted constrained state. The drawback of the aforementioned method is the dependence on the innovation sequence vector assuming few outliers and several redundancies. If the measurements do not have significant redundancy or they are contaminated by biased errors, these methods will perform poorly. To overcome this limitation, we use maps to generate extra constraints and adaptively estimate the GNSS noise covariance matrix. The proposed method does not depend on the innovation sequence and, hence, it is capable of handling GNSS biased errors. Furthermore, it does not need several consistent redundant measurements and, hence, can be applied to any integration

4 scheme including loosely-coupled scheme, which are specifically considered in this paper. IV. THE PROPOSED MAP-AIDED GNSS ADAPTIVE FUSION FRAMEWORK A. Vehicle Dynamic Model The utilized reference frames in this work is described in Table and shown in Figure 6. In this work, the state vector consists of vehicle s position, velocity, orientation, and IMU sensors biases as follows: ( l) ( l) lb x k pk ; vk ; k ; ba ( t); b ( t) () l p k l (7) where N T kek Dk is the north-east-down (NED) () position vector in local navigation frame, v l k vnvevd is lb the velocity vector in local navigation frame, k k k k is the vehicle s orientation with respect to local-level navigation represented in Euler angles (yaw, pitch, roll) [3] [5] [7], b () t b b b and b () t b xb yb z a ax ay az are biases of IMU accelerometers and gyroscopes measurements respectively. V. Frame b: body frame Table. Coordinate Reference Frames Definition Origin: Vehicle center of mass X: Longitudinal (forward) direction Y: Transversal (lateral) direction Z: Down (vertical) direction where is the accelerometer measurements in bodyframe, R l b t ( b) a ( b) t is the gyroscope measurements in body-frame, is the direction cosine matrix [3] [] [7] that represents the vehicle s orientation, () l EL is the rotation rate of the locallevel navigation frame l with respect to the ECEF frame due to motion on the ellipsoid surface of Earth (known as transport rate [3]) and () l IE is the earth rotation rate of ECEF frame with respect to the inertial navigation frame. () l IE is often a small value compared to MEMS gyros noise which can be ignored depending on the application. Sensors errors are also modeled using a Gauss-Markov [8] [3] [7] random process as follows: b t b t w t a ( ) a a ( ) a a ( ) b t b t w t ( ) ( ) ( ) where a, a, and () () are time constants and covariance of Gauss-Markov process models [3] [5] of accelerometer and gyroscopes biases respectively. To use the EKF, the dynamic model is linearized to obtain a 5 state linear error model in the form described in (3). The linearization and details of numerical implementation of the vehicle s system dynamic model can be found in [5] [7]. In the remainder of the paper, the vehicle state calculated by the system dynamic model will be referred as dead-reckoning (DR). l: local-level frame Earth- Centered Earth-Fixed (ECEF) frame Origin: Vehicle centre of mass X: True north direction Y: East direction Z: Down vertical direction Origin: Center of the Earth Z: extends through the North Pole. X: passes through the intersection of the equatorial plane and the prime meridian. Y: completes the right-hand coordinate system in the equatorial plane. For a rigid object in 3D, motion model f (.) is given by [5] [7]: l INS p t v t ( l) ( ) INS () v t R t a t b t ( l) l ( b) INS b a l b g t v t ( l) ( l) ( l) ( l) EL IE INS l b R t R t ( b) ( l) ( l) t b () t EL IE (8) (9) () Figure 6. Reference Frames B. Map-matching using Hidden Markov Models The simplest map-matching method is called point-to-curvematching [9]. It is performed by searching for closest road segments within a distance threshold from the current vehicle s position. The distance is commonly calculated between the

5 vehicle s position and its projection on the map segment. However, this approach is sensitive to state estimation errors and it fails in intersections, joins, branches, or dense parallel roads. For example, Figure 7 shows a situation where biased GNSS position measurements exists and the wrong map segment is selected because of the dependence on the distance metric only (D is less than D). Therefore, the distance measure alone is not enough to select the right segment. If the legal direction of motion of the map segment is matched with the vehicle heading, accuracy can be improved. However, during turns the vehicle heading is in between one or more segments and it may not favor the correct segments. A solution for this issue could be the utilization of a Particle Filter (PF) [], where multiple hypotheses can be considered simultaneously, and evaluated later when more measurements become available. In this paper, we propose an alternative technique by keeping a recent portion of the vehicle motion history and using it in the matching criteria. This strategy is known as curve to curve matching [9] []. The matching criteria can include map topology (connectivity) constraints to exclude unlikely map segments. In order to incorporate these constraints in a well-defined, consistent, mathematical framework, a HMM-based algorithm is applied. A Markov model is a stochastic model that describes sequence of states S s, s, s3..., sn depends only on. Process []. The transition from to sk s k S s k, where is called a Markov s k modeled by a conditional transition probability given by: a p s k S s k S ij i j can be (3) This conditional transition probability forms a Markov model. The probability of any observed sequence under a certain Markov Process and Markov model M is given by:,, 3..., N p s i p s i s i p s s s s N M (4) If the states are not directly observable (hidden) but can be indirectly observed through a sequence of outputs,, 3..., x x x x N, the process is called a Hidden Markov Process. The HMM in this case is characterized by the transition probability given in (3) and an emission probability that represents the probability that a given state s k generates an output x k : p s k x k (5) Figure 7. Wrong map-segment selection in intersection In general, the consideration of the following constraints significantly improves map-matching accuracy: When projecting a single vehicle state to multiple segments, the segments that have legal direction of motion closest to the vehicle s current heading should be preferred. A transition from one segment to another should not violate the legal and logical connectivity between road segments (map topology constraints). A transition from one segment to another should not violate the vehicle s reasonable range of dynamics. For example, in normal operating scenarios, a vehicle cannot change heading by 8 o in one second. The spatial characteristics of a sequence of vehicle states must be consistent with the geometry of the selected road segments. A fundamental problem of HMM is that; given a sequence of x, x, x 3..., x N, what is the best outputs sequence of states that explains the observed outputs. This problem is solved by selecting the sequence of states that maximize the HMM probability as follows: S arg max,,... S s s s N N i p si xi p s x p s i s i (6) This estimation process is called decoding and it is solved using the Viterbi Algorithm []. To develop a robust mapmatching framework, the vehicle pose history, roads geometry, and map topology constraints must be considered. Therefore, the emission and transition probabilities of a HMM are formulated such that they reflect all the aforementioned constraints. The HMM-based framework is illustrated in Figure 8.

6 Figure 8. Hidden Markov Model for Vehicle s State Map-matching In the proposed work, the length of the processed buffer of the vehicle s state is determined based on the travelled distance. The aim is to accumulate a reasonable geometric knowledge about the trajectory segment that enables the HMM to accumulate enough geometric and topological constraints to be able to select the correct sequence of road segments in difficult intersections, joins, and exit/entry roads. C. Adaptive GNSS Measurement Fusion The proposed adaptive method estimates the GNSS position errors as a conditional Gaussian probability distribution conditioned on the selected road map segment and mapmatched position (the projection of a vehicle s position on selected road segment). In other words, we would like to estimate the GNSS position noise distribution given the mapmatched position error distribution. To achieve this goal, the map-matched position is used as a supplemental position update to the filter in addition to GNSS position and velocity updates. Therefore, the measurement error model is defined as follows: ( l) ( l) pk p GNSS k INS v v H x ( l) ( l) pk p MAP k INS ( l) ( l) k GNSS k INS k where H is defined accordingly. Let e k (7) represents errors vector of GNSS position, velocity and map-matched position measurements. The error vector e is modelled as a multivariate Gaussian distribution [3] [] as follows: e N, R k 9x 9x9 R O O gnss _ pos gnss _ map T O3 x3 gnss _ vel 9x9 gnss _ map map _ pos O3 x3 k (8) (9) where 9x is a 9 by zeros vector, gnss _ pos matrix which represents GNSS position covariance, is a diagonal gnss _ vel is a diagonal matrix which represents GNSS velocity map _ pos covariance, and is covariance matrix for mapmatched position. The sub-matrices gnss _ map model the correlation between GNSS position and map-matched position measurements. Since GNSS velocity estimation is based on Doppler frequency shifts [3] [], it is less affected by multipath. Therefore, we assume no correlation between GNSS velocity measurements and GNSS/map position measurements which is indicated by the 3 by 3 zero matrices in the definition of R. Since GNSS velocity and map-matched position are not vulnerable to multipath errors, the values of map _ pos gnss _ vel and are assumed fixed and estimated by proper tuning. Therefore, the problem is reduced to the estimation of the GNSS position covariance matrix map _ pos gnss _ pos given gnss _ vel and. Because the measurement errors are assumed to be drawn from a multivariate Gaussian with zero mean and covariance R, Bayesian inference can be used to estimate gnss _ pos as follows [3] [4]: gnss _ pos T gnss _ vel 3 3 x gnss _ map map _ pos gnss _ map where (3) is a 3 by 3 diagonal matrix which represents default GNSS position covariance values (set to a large value that reflects the high degree of uncertainty of GNSS position measurements) and the correlation matrix by: x map _ pos y z x y z T gnss _ map is defined (3) The vector is calculated using an exponential covariance kernel function [3]: T T [ x y z ][ x y z ] (3) x y z e where j x, y, z is the difference between the j th map-matched position component and the corresponding GNSS position component as measured in the vehicle s body frame, and

7 [ ] T x y z is the squared distance vector. The parameters x y z and x y z are hyper-parameters of the kernel function. The hyper-parameters are chosen to control the covariance values such that it is stronger in the vehicle s lateral direction and weaker in the vehicle s longitudinal direction. This is consistent with the nature of the map matching constraints which reduces uncertainty more in the lateral direction. The intuitive interpretation of equation (3) can be explained as follows. The large default GNSS position covariance reflects large uncertainty about GNSS position measurements. If a GNSS position update is close to the map matched position, the correlation matrix be large and the values of gnss _ pos gnss _ map will will be small and, consequently, GNSS position update will be trusted by the integration filter. If GNSS position update is contaminated by errors (either biased or zero-mean), the correlation matrix gnss _ map will be small and the values of gnss _ pos will be large and, consequently, this GNSS position update will be rejected by the integration filter. If all sub-matrices are kept diagonal, the matrix operation in (3) can be decomposed into separate equations as follows: j j j e gnss pos gnss pos j x, y, z jo map _ pos j (33) The relationship in (33) is demonstrated in Figure 9. The hyperparameters and the map-matched position covariance are chosen to control the shape of the relationship given in (33). Figure 9. Relationship between the adaptive variance of a single GNSS position component vs. difference from the corresponding map-matched position component Figure shows the relationship between the covariance value and the distance difference for one position component for different values of j, 4 and variance (m ) j set to gnss _ pos jo set to, map _ pos j set to -5 5 distance (m) Figure. Variance vs. GNSS-map distance difference for a single position component. D. Algorithm Steps and Implementation We estimate the active map segments using the history of vehicle states estimated by DR only. Otherwise, noisy GNSS will likely lead to incorrect selection of map segments. DR needs to be accurate enough to assure proper map segment selection and therefore, the EKF needs to have converged to a stable state where proper IMU biases are well-estimated. In addition, DR cannot estimate the initial pose. To address this issue, the proposed algorithm requires an acquisition phase first before applying the proposed adaptive tracking scheme. The acquisition phase is performed to assure convergence of the EKF in open-sky conditions. During this phase, the GNSS position/velocity updates are fused in the traditional looselycoupled fashion where the values of covariance matrix are fixed at a best tuned value. The buffered vehicle states estimated by INS/GNSS fusion are used by the proposed HMM-based mapmatching technique without taking updates or feedback from the map-matched positions. Convergence of the EKF is measured by the consistency between the predicted position P INS and the GNSS position updates P GNSS and the distance between INS/GNSS fusion output P INS / GNSS and mapprojection positions P MAP values are calculated as follows:. Therefore, two innovation sequence M GNSS INS GNSS (34) M i e P P Beta =. Beta =. Beta =.5

8 M MAP INS / GNSS MAP M i e P P (35) If e GNSS and e MAP are within a threshold, the acquisition is declared and a tracking phase is initiated where the adaptive GNSS fusion method is applied and the tracking phase is activated. The steps of the tracking phase are described as follows: ) Perform dead-reckoning and construct a buffer of the vehicle s state. Because this buffer is relatively short and the EKF has converged to reliable IMU biases, the DR solution is accurate enough to enable robust HMM-based map-matching. ) Apply the HMM-based map matching step and calculate the map-matched position. The map-matched position is calculated as the projection of the DR position on the selected road segments. 3) Apply the map-matching position updates to the EKF using a strong covariance value in the vehicle s lateral direction and a weaker covariance value in the vehicle s longitudinal direction. The reason for this is that the map-matching provides stronger correction in vehicle s lateral direction and higher uncertainty in vehicle s longitudinal direction as Figure indicates. 4) At the epochs at which GNSS updates are available, apply the adaptive covariance estimation method to adaptively estimate GNSS measurement noise covariance. The adaptive GNSS error covariance estimation is further depicted in Figure which shows two GNSS position updates weighted differently according to its proximity to map-matched position. Because map segments provide higher uncertainty in longitudinal direction, the estimated GNSS position measurement noise covariance will also reflect this fact. Figure. Adaptive GNSS error covariance method VI. EXPERIMENTAL WORK A. Experimental Setup The proposed system was tested on a mini-van platform, where a Ublox EVK-7 GNSS receiver with an automotive grade IMU from YOST Labs were connected to a Toshiba laptop running Windows with the developed sensor fusion engine installed. The experimental setup is shown in Figure 3. B. Testing Trajectories Two testing trajectories were collected; one in the city of Waterloo and another in downtown Toronto, as depicted in Figure 4 and Figure 5, respectively. The first testing trajectory is mostly open-sky with accurate GNSS. Simulated errors (zero-mean and biased) were introduced to test the proposed adaptive method. The second testing trajectory include natural biased GNSS errors. It was designed to include 3 regions. Region is a relatively open-sky portion where the filter converges with proper estimation of IMU biases and error covariance matrix. Region contains urban canyon sections where significantly biased GNSS errors occurred. Region 3 has exit/entry roads with dense intersections and overlapped map segments. In region 3, testing of the HMM-based mapmatching during complete GNSS outage was testing in a challenging area that contains overlapped road intersections, bridges, and exit/entry lanes. Figure. Illustration of Map-matched position and map-matched position update measurement error covariance Figure 3. Experimental Setup

9 Longitude Error Covariance STDV(m) Altitude Error Covariance Figure 4. Testing Trajectory#, mostly open-sky. Simulated GNSS noise is used to test the proposed method. STDV(m) Heading Error Covariance STDV(degrees) Figure 5. Testing Trajectory#. Contains natural biased GNSS errors. C. Implementation The sensor fusion technique including data logging, the EKF equations, 3D motion models, system error model, measurements model, HMM, Viterbi algorithm, and road map segments processing were implemented in ANSI C. Data logging was performed through two threads; one for GNSS and the other for IMU. The GNSS was logged at Hz, while the IMU was recorded at 5 Hz. The map data was prepared offline and cached in a binary file for easy and quick access. The data was collected in the field and post-processed. D. Results ) Acquisition Phase In this subsection, the results of the Trajectory# are discussed first. The convergence of the EKF during the acquisition phase can be seen in Figure 6 - Figure 9. Since it is not directly observable, the azimuth angle error convergence takes longer than the other states, as can be seen in Figure 6 and the effect of this delay is also observable in Figure 9 in the bias convergence of the vertical gyroscope. During the acquisition phase, the GNSS position covariance estimated by the receiver is scaled and used in the integration filter. Figure 8 shows the scaled GNSS D horizontal error covariance applied during the acquisition. The D horizontal position error during the acquisition phase is shown in Figure 7. After the filter convergence, the tracking phase is triggered where the proposed adaptive GNSS position noise covariance estimation mechanism is activated. D horizontal error(m) STDV(m) Figure 6. EKF error state covariance of horizontal position and heading D horizontal error(m) 5 5 Figure 7. D position horizontal error during acquisition phase..5.5 GNSS Position Measurement Noise (m) 3 4 Figure 8. Scaled D horizontal GNSS position measurement noise covariance during acquisition phase as reported by the receiver.

10 Acc Biases(m/s) Gyro Biases(degree/s) Figure 9. Accelerometer and Gyroscope Biases during acquisition phase ) Tracking Phase During the tracking phase, and under reliable GNSS position updates, the performance is comparable to the standard EKF scheme as can be seen from the error values in Table. The slightly higher errors of the adaptive scheme under reliable GNSS is because the ground truth is taken from GNSS. Since the adaptive scheme fuses the map-matched positions, the solution tends to follow the map segments. To test the proposed adaptive scheme under different noise conditions, simulated position errors have been added to the GNSS measurements in four sections for seconds each. First, zero-mean random noise with 3m standard deviation is applied to the D GNSS position measurements and the performance is evaluated with and without the proposed adaptive fusion method and the D position errors are plotted in Figure. Table. Position errors under reliable GNSS Standard EKF Adaptive Scheme D POS.7m.667m D Max POS Error m D horizontal error(m) Accelerometers Biases Bias AccX Bias AccY Bias AccZ Gyroscopes Biases D horizontal error(m) Bias GyroX Bias GyroY Bias GyroZ Standard EKF Adaptive EKF Figure. D Horizontal error under zero-mean Gaussian GNSS position noise of 3m standard deviation. Similarly, a random biased error of a 3m mean and 5m standard deviation was added to GNSS position measurements and performance is evaluated with and without the proposed adaptive approach, with errors are plotted in Figure. Error values for all four testing sections under the aforementioned tests are summarized in Table 3 and Table 4. It can be seen from Figure and Figure that the effect of unhandled GNSS position errors can be quite large, and how the proposed mapaided adaptive fusion scheme significantly reduce this effect. A snap-shot of the trajectory under the zero-mean noise test and biased noise test is shown in Figure and Figure 3 respectively. Figure 4 shows an intersection portion of the trajectory under the zero-mean noise test where the selected road map segments are highlighted in green. D horizontal error(m) Figure. D Horizontal error under biased-mean Gaussian GNSS position noise of 3m. Table 3. Map-aided adaptive EKF performance under zero-mean Gaussian position noise Zero-mean Noise Test D POS D Max POS Error Zero-mean Noise Test D POS D Max POS Error D POS Improvement D MAX Improvement Standard EKF Section Section Section3 Section Map-aided Adaptive EKF Section Section Section3 Section % 74.85% 7.76% 85.7% 89.8% 79.59% 77.39% 85.85% Table 4. Map-aided adaptive EKF performance under biased Gaussian position noise Biased Noise Test D POS D Max POS Error Biased Noise Test D POS D horizontal error(m) Standard EKF Adaptive EKF Standard EKF Section Section Section3 Section Map-aided Adaptive EKF Section Section Section3 Section

11 D Max POS Error D POS Improvement D MAX Improvement % 88.3% 86.% 96.36% 97.% 86.3% 77.83% 95.34% measurements with reliable HDOPs were reported by the receiver. Portion of the processed trajectory are shown in Figure 5 and Figure 6 where the biased GNSS measurements are well-mitigated by the proposed algorithm. In Figure 7, testing of the HMM-based map-matching during complete GNSS outage was applied in a challenging area that contains overlapped road intersections, bridges, and exit/entry lanes. This figure shows the robustness of the developed mapmatching technique even under complete GNSS outage. Figure. Zero-mean random GNSS outliers rejected by the proposed mapmatched adaptive fusion system. GNSS is in blue and map-aided adaptive sensors fusion is in red. Figure 5. Natural Biased GNSS measurements mitigation in Downtown Toronto. GNSS in blue and map-aided adaptive sensors fusion is in red. Selected map segments are highlighted in green. Figure 3. Biased GNSS outliers rejected by the proposed map-matched adaptive fusion system. GNSS is in blue and map-aided adaptive sensors fusion is in red. Figure 6. Natural Biased GNSS measurements mitigation in Downtown Toronto. GNSS in blue and map-aided adaptive sensors fusion is in red. Selected map segments are highlighted in green. Figure 4. Zero-mean random GNSS outliers rejected by the proposed mapaided adaptive fusion system. GNSS is in blue and map-aided adaptive sensors fusion is in red. Selected map segments are highlighted in green. Trajectory# The proposed method was tested on another trajectory in downtown Toronto where natural biased GNSS position Figure 7. Selected map segments (green) in an intersection and an entry lane. Road network is shown in orange color.

12 VII. CONCLUSION AND FUTUREWORK This paper introduced a map-aided adaptive sensors fusion technique that can be applied for any map-enabled navigation systems. The main advantages of the proposed framework are the efficient improved map-matching technique using HMM, the feedback from map-matched position as EKF updates with proper error covariance that consider strong confidence in the lateral direction and weaker confidence in the longitudinal direction, and finally the adaptive GNSS error covariance estimation method that is able to mitigate both zero-mean and biased GNSS errors. Real-road testing in Waterloo and downtown Toronto shows reliable performance that is robust under significant zero-mean and biased GNSS measurements noise. Furthermore, the HMM-based map-matching mechanism showed reliable and robust performance in selecting the correct road segments in difficult road intersections and entry lane locations. It is understood that the proposed method assumes reliable map coverage and continuous map-constrained motion. In addition, the matching accuracy is currently bounded to road-center level. Therefore, the following future work points will be considered; reduced information from map matching in situations such as parking lots/garages, and increased precision through the integration with lane marking information if available to further enhance the precision of the map matching position estimation to the lane level. VIII. REFERENCES [] P. Misra and P. Enge, "Global Positioning System, Signals, Measurements, and Performance," Ganga- Jamuna Press,. [] William J. Hughes Technical Center, "Global Positioning System (GPS), Standard Positioning Service (SPS), Performance Analysis Report #86," William J. Hughes Technical Center, NSTB/WAAS T&E Team, July 3, 4. [3] J. Farrell, Aided Navigation GPS with High Rate Sensors, New York: McGraw-Hill, 8. [4] El-Rabbany, Introduction to GPS: The Global Positioning System, Second Edition, Artech House, 6. [5] P. D. Groves, Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems, Artech House, 3. [6] P. D. G. a. M. K. Z. Lei Wang, "Multi-Constellation GNSS Performance Evaluation for Urban Canyons Using Large Virtual Reality City Models," THE JOURNAL OF NAVIGATION, vol., no. 65, p ,. [7] A. Chambers, S. Scherer, L. Yoder, S. Jain, S. Nuske and S. Singh, "Robust Multi-Sensor Fusion for Micro Aerial Vehicle Navigation in GPS-Degraded/Denied Environments," in 4 American Control Conference, Portland, OR, June 4. [8] M. M. Atia, S. Liu, H. Nematallah, T. B. Karamat and A. Noureldin, "Integrated Indoor Navigation System for Ground Vehicles With Automatic 3-D Alignment and Position Initialization," IEEE Transactions on Vehicular Technology, vol. 64, no. 4, pp. 79-9, 3 January 5. [9] A. Vu, A. Ramanandan, A. Chen, J. A. Farrell and M. Barth, "Real-Time Computer Vision/DGPS-Aided Inertial Navigation System for Lane-Level Vehicle Navigation," IEEE Transactions on Intelligent Transportation Systems, vol. 3, no., pp , March. [] R. Toledo, D. Betaille, F. Peyret and J. Laneurit, "Fusing GNSS, Dead-Reckoning, and Enhanced Maps for Road Vehicle Lane-Level Navigation," IEEE Journal of Selected Topics in Signal Processing, vol. 3, no. 5, pp , 9. [] R. J. Elliott, L. Aggoun and J. B. Moore, Hidden Markov Models: Estimation and Control, Springer, Business & Economics, 995. [] S. Sarkka and A. Nummenmaa, "Recursive Noise Adaptive Kalman Filter by Variational Approximation," IEEE Transactions on Automatic Control, vol. 54, no. 3, pp , 9. [3] Z. Zhou, Y. Li, J. Liu and G. Li, "Equality Constrained Robust Measurement Fusion for Adaptive Kalman- Filter-Based Heterogeneous Multi-sensor Navigation," IEEE Transactions on Aerospace and Electronic Systems, vol. 49, no. 4, p. 46, 3. [4] W. Ding, J. Wang and A. Almagbile, "Adaptive Filter Design for UAV Navigation with GPS/INS/Optic Flow Integration," in International Conference on Electrical and Control Engineering,. [5] A. Werries and J. M. Dolan, "Adaptive Kalman Filtering Methods for Low-Cost GPS/INS Localization for Autonomous Vehicles," Research Showcase, School of Computer Science, Carnegie Mellon University, May 6. [6] V. Peteroukhin, W. Vega-Brown, N. Roy and J. Kelly, "PRPBE-GK: Predictive Robust Estimation using Generalized Kernels," in the IEEE International Conference on Robotics and Automation (ICRA 6), Stockholm, Sweden, May 6. [7] P. G. Savage, Strapdown Analytics - Second Edition, MN, USA: Strapdown Associates, Inc.,. [8] H. Durrant-Whyte, "Introduction to Estimation and the Kalman Filter," Australia,, p. Australian Centre for Field Robotics. [9] M. Hashemi and H. A. Karimi, "A critical review of real-time map-matching algorithms: Current issues," Elsevier, Computers, Environment and Urban Systems, vol. 48, p , 4. [] R. Toledo, D. Betaille and F. Peyret, "Lane-Level Integrity Provision for Navigation and Map Matching With GNSS, Dead Reckoning, and Enhanced Maps," IEEE Transactions on Intelligent Transportation Systems, vol., no., pp. -,.

13 [] S. Haibin, T. Jiansheng and H. Chaozhen, "A Integrated Map Matching Algorithm Based on Fuzzy Theory for Vehicle Navigation System," in International Conference on Computational Intelligence and Security, Guangzhou, 6. [] L. K. Balivada and K. P. Raju, Optimization Techniques of Viterbi Algorithm: Performance Analysis of Different Algorithms, LAP LAMBERT Academic Publishing, May,. [3] C. E. Rasmussen and C. K. I. Williams, Gaussian Processes for Machine Learning, The MIT Press, 6. [4] M. Eaton, Multivariate Statistics: A Vector Space Approach, vol. 53, Wiley and Sons, July 983. Dr. Mohamed M. Atia received B.S. and M.Sc. degrees in computer systems from Ain Shams University in and 6 and a Ph.D. in electrical and computer engineering from Queen s University at Kingston in 3. He held several full-time positions in industry developing algorithms for natural language, speech recognition, and multi-sensors navigation, guidance, and mobile mapping systems. He is currently NSERC Postdoc Fellow in The University of Waterloo. Dr. Atia is a recipient of the Alberta Innovate Association Award in, the IEEE excellence in PhD research in 3, Mitacs Elevate Industrial Postdoc Award in 4, NSERC PDF Award in 5, and Queen s University Teaching Award in 6. Prof. Waslander received his B.Sc.E.in 998 from Queen's University, his M.S. in and his Ph.D. in 7, both from Stanford University in Aeronautics and Astronautics. He was a Control Systems Analyst for Pratt & Whitney Canada from 998 to. In 8, he joined the Department of Mechanical and Mechatronics Engineering at the University of Waterloo in Waterloo, ON, Canada, as an Assistant Professor. He is the Director of the Waterloo Autonomous Vehicles Laboratory (WAVELab, His research interests are in the areas of autonomous aerial and ground vehicles, simultaneous localization and mapping, nonlinear estimation and control, and multi-vehicle systems.