DTM-Aided Adaptive EPF Navigation Application in Railways

The diverse operating environments change GNSS measurement noise covariance in real time, and different GNSS techniques hold different measurement noise covariance as well. Mismodelling the covariance causes undependable filtering results and even degenerates the GNSS/INS Particle Filter (PF) process, due to the fact that INS error-state noise covariance is much smaller than that of GNSS measurement noise. It also makes the majority of existing methods for adaptively adjusting filter parameters incapable of performing well. In this paper, a feasible Digital Track Map-aided (DTM-aided) adaptive extended Kalman particle filter method is introduced in GNSS/INS integration in order to adjust GNSS measurement noise covariance in real time, and the GNSS down-direction offset is also estimated along with every sampling period through making full use of DTM information. The proposed approach is successfully examined in a railway environment, and the on-site experimental results reveal that the adaptive approach holds better positioning performance in comparison to the methods without adaptive adjustment. Improvements of 62.4% and 14.9% in positioning accuracy are obtained in contrast to Standard Point Positioning (SPP) and Precise Point Positioning (PPP), respectively. The proposed adaptive method takes advantage of DTM information and is able to automatically adapt to complex railway environments and different GNSS techniques.


Introduction
The Global Navigation Satellite System (GNSS) has been broadly utilized in aviation for decades. Subject to RAMS (Reliability, Availability, Maintainability and Safety) requirements, as well as economic reasons of safety-critical applications in railways, GNSS has not been popularly accepted by rail authorities. Due to its global capability of high accuracy and availability, GNSS, as a localization sensor, nevertheless plays a strategic role in the train control system of the next generation, e.g., the European Train Control System Level 3 (ETCS-3), the Chinese Train Control System (CTCS)and Positive Train Control (PTC) [1][2][3], and some pioneer lines have been deployed worldwide to testify the performance in actual train operating environments [4].

GNSS/INS Extended Kalman Particle Filter
The GNSS/INS EKF and an improved Sampling Importance Resampling (SIR) filter are described in this section. The EKF will be used in Section 3 to assist the SIR filter to estimate and update GNSS measurement noise covariance matrix; thus, a better described density can be obtained to guide the importance sampling of the SIR filter, which is described in Section 3 as well.

System Model
The most common GNSS/INS fusion method is the loosely-coupled integration of 15 error-state vector x k [10], which is given by: where δψ e eb denotes angular-rate error, δv e eb represents velocity error, δr e eb is position error and b a and b g denote accelerometer biases and gyro biases, respectively. The error-state vector-based system continuous model is thus: where Ω e ie is the skew-symmetric matrix of the angular rate,Ĉ α β denotes the coordinate transformation matrix, which transforms a vector from the β coordinate frame to the α coordinate frame, g0 is the surface gravity,r e eb denotes an estimate of r e eb , which is the INS Cartesian position of the origin of Frame b with respect to the origin of Frame e, resolved about the axes of Frame e. r e eS is the distance of a point S with respect to the Earth's centre.f b ib is the IMU measurement specific force along the body-frame resolving axes.L b denotes INS latitude estimate. G and w s denote the system noise distribution matrix and the system noise vector, respectively.
The system state vector transition matrix is given by: where τ s denotes the Kalman Filter interval, not the IMU output interval.

Measurement Model
GNSS/INS integration uses GNSS position r e ebG and velocity v e eb measurements, thus: and measurement matrix H k is given by: The EKF algorithm comprises the following steps [12]: (a) Calculate the transition matrix, Φ k−1 ; (b) Calculate the system noise covariance matrix, Q k−1 ; (c) Propagate the state vector estimate fromx + k−1 tox − k−1 : (d) Propagate the error covariance matrix from P + k−1 to P − k : (e) Calculate the measurement matrix H k ; (f) Calculate the measurement noise covariance matrix R k ; (g) Calculate the Kalman gain matrix K k : (h) Calculate the innovation δz k ; (i) Update the state vector estimate fromx − k tox + k : (j) Update the error covariance matrix from P − k to P + k :

Improved Sampling Importance Resampling Filter
The SIR filter, also known as the bootstrap filter [35], along with the Sequential Importance Sampling (SIS) filter are basic particle filters. The SIS filter turns to an SIR filter by choosing the importance density to be the transitional prior; in addition, unlike the SIS filter conducting resampling if and only if a significant degeneracy is observed, SIR performs the resampling step in everyfiltering recursive cycle.
Firstly, let us introduce X k = {x j , j = 0, ..., k}, which represents the system state sequence up to time k, X i j = {x i j , j = 0, ..., k, i = 1, ..., N}, which is the sequence of N sampling points at time j. Suppose the multidimensional integral I(X k ) is given by: (12) where x ∈ R n x . Based on Monte Carlo (MC) integration, it is possible to draw N 1 sample sequence X i k ; therefore, the MC estimate of integral I(X k ) is the sample mean: where X k (x) = x k · π(x k ) and π(x k ) is a probability density function. The law of large numbers reveals that I N (X k ) will converge to I(X k ). Unfortunately, it is usually impossible to sample system states X i k properly, which means we cannot use Equation (13) to estimate system states at time k. The discovery of importance resampling and resampling techniques paves the way for the success of PF. At an importance resampling stage, it supposes samples from a similar density q(x k ) can be drawn. Then, Equations (12) and (13) can be rewritten respectively as: where the importance weights are given by:ω Furthermore, applying importance sampling in the Bayesian framework, thus π(x k ) becomes the posterior density p(x k |Z k ), where Z k denotes measurements up to time k. Therefore, Equations (15) and (16) can be rewritten as [36,37]: where ω i k denotes normalized weights, i.e., ∑ i ω i k = 1, and X i k represents samples drawn from an importance density q(X k |Z k ).
Ideally, the importance density should be p(X k |Z k ); thus, applying q(X k |Z k ) causes the sample degeneracy phenomenon due to the increase of the variance of importance weights [37]. A resampling is introduced to eliminate low importance weight samples by a random measure. It selects some high weight samples to replace low ones.
The most popular transitional prior is given by: Substitution of Equation (19) into (18) yields: However, because the resampling is applied in every SIR filter recursive cycle, it reveals that all particles have equal weights; thus: In the case of GNSS/INS fusion, if Equation (19) is applied, Equation (21) causes particles to degenerate rapidly. An improved method is to use EKF to approximate importance density p(X k |Z k ).
The key is that EKF provides mean (X k ) and variance (P k ) to guide importance resampling:

DTM-Aided Adaptive GNSS/INS EPF
In this section, the proposed DTM-aided GNSS covariance matrix estimation approach is firstly introduced, which is the key of the adaptive EPF algorithm, and then is combined with GNSS/INS EKF and the SIR filter explained in Section 2. A detailed DTM-aided adaptive GNSS/INS EPF architecture will be finally described.

DTM-Aided GNSS Covariance Matrix Estimate
The ultimate goal of different adaptive algorithms is to achieve the ability of estimating unknown parameters correctly and in real time. In the context of estimating the GNSS noise covariance matrix in a GNSS/INS integration, it usually involves adjusting the unknown parameter based on INS. In the rest of this paper, a high-definition DTM representing a central line of tracks is introduced to model the two parallel tracks. Thanks to the nature of the train navigation application, the train is constantly running on the tracks. That is, no matter what kind of GNSS technique is adopted and how the surrounding environment changes, the GNSS equipment mounted on the train is constantly located in a line described by DTM, if the height between GNSS and tracks is known and deducted previously. Moreover, assume the GNSS noise errors in Cartesian coordinates are independent and identically distributed. It is possible to construct an adaptive estimation algorithm, which is reported in Figure 1. − → D1 represents a vector from r k to p G k ; similar definitions of vectors − → D2 to − → D6 have been illustrated in Figure 1. Figure 1 also shows the direction that the train is running and the alternative positions of point r k , r k−1 , which are denoted by r a k , r a k−1 . Therefore, the uncertainty of the r k , r k−1 positions is described as: It is further assumed that the distance INS reckons along DTM at time k is approximate to the GNSS measurement distance along DTM between time k and k − 1. It gives: Substituting Equation (24) into (23) gives: If D1 and D2 follow the same zero-mean Gaussian distribution and are independent, σ 2 a represents GNSS measurement noise variance along the DTM direction, σ 2 v is the GNSS measurement noise variance orthogonal to DTM on the plane of tracks, σ 2 vv denotes the GNSS measurement noise variance orthogonal to DTM in the direction vertical to the plane of tracks, point p G,P k is the projection of the GNSS positioning point on the plane of tracks and − → D8 denotes a vector from point p G,P k to r G k . Therefore: Equations (26) and (27) combined with (28) provide a way of estimating GNSS measurement noise covariance matrix R k . In comparison to other estimate algorithms, DTM considered as a sensor gives real GNSS measurement errors in two vertical directions. Meanwhile, because INS short-term error is relatively stable and smaller than that of GNSS, it is reasonable to estimate GNSS measurement noise along DTM by using INS information. A universal GNSS measurement noise variance σ 2 G is given by:

DTM-Aided Adaptive GNSS/INS EPF Architecture
The proposed algorithm is based on the particle filter. In order to adjust GNSS measurement noise covariance, the DTM and covariance estimate modules are introduced. The DTM module provides position points r DTM k and interpolates these points in support of calculating p G k . The covariance estimate module collects information from the GNSS, INS and DTM modules to estimate R k in real time. The GNSS/INS EPF module, as the main fusion module, accounts for the particle filter, e.g., importance resampling. The closed-loop architecture is illustrated in Figure 2. In the closed-loop configuration, the average values of output particles in every recursive process, which represent estimated accelerometer, gyro, position, velocity and attitude errors, are used to correct IMU measurements, respectively INS. The IMU output frequency is usually higher than that of GNSS. k denotes the time when the GNSS measurement arrives. The proposed Algorithm 1 is given below. Update i(th) particle: Estimate GNSS measurements: Zpre (i) 19 Update i(th) particle weight based on Equation (18): Resampling: 23 Residual resampling method [38] 24 end mod is modular operation; M denotes sample size; Z k represents GNSS measurements at time k. During time k and (k − M), the algorithm collects samples to estimate the GNSS measurement noise covariance matrix. A step of importance resampling is followed, which is the key of the improved EPF algorithm. It takes advantage of EKF and makes use of the EKF estimated particle mean and standard deviation to guide the importance resampling process. This means the algorithm uses EKF to help generate samples, which are approximate to the desired posterior density. randn(15) is a 15 × 15 diagonal matrix, in which the element is a random variable following the standard normal distribution, X

Experimental Environment
This section describes the test line selected for the on-site experiments. In order to verify the performance of the proposed adaptive algorithm, some tests were conducted on the Test Ring of the China Academy of Railway Sciences in Beijing. As shown in Figure 3, the test ring is around 9 km in length, and the speed of the train can be as high as 180 km/h. The experimental equipment includes the trackside reference station and onboard system, which are illustrated in Figure 4. The IMAR-FSAS IMU records raw IMU data, and unicorecomm UR 380 multi-frequency GNSS receivers are adopted as the base station receiver, as well as the rover. The reference station is set in static mode, so that the RTK positioning solution can be obtained as the ground truth. Post-processing the rover data, accordingly, the DGNSS, PPP and standard point positioning (SPP) results are analysed to assess the proposed algorithm. As DTM plays a key role in it, in order to create an accurate digital track map, RTK results are interpolated to organize DTM, which consists of a series of point coordinates.

Results
The test train was driven along the tracks with a maximum speed of 65 km/h; meanwhile, a deceleration and an acceleration in the range of 65 km/h to 0 km/h were included along the period of running. GNSS measurement output frequency was 1 Hz, Figure 5 reveals the SPP, PPP and DGPS positioning errors with respect to the RTK solution. It clearly shows that SPP had a meter-level positioning error, and PPP and DGPS obtained a decimetre-level one. DGPS had the best performance, especially when the test train was static during the operation. Without post-processing the results, usually prior GNSS measurement noise variances (σ 2 ) were applied to model SPP, PPP and DGPS measurement noises, which are metre-level and decimetre-level, respectively. The positioning results of GNSS/INS EPF with nominal σ 2 (for SPP, PPP and DGPS, σ 2 are equal to 10, 1 and 1 respectively) are illustrated in Figure 6. It shows that the nominal variance values were feasible to be adopted in the filtering process. In general, these variance values are able to reflect positioning errors.
Another fact we want to investigate is how different variance values will affect EPF results. Regarding SPP, typical positioning noise variance, values of 15, 10, 5 and 1 were fed into EPF, while 5, 3 and 1 were adopted for PPP and DGPS; thus, the filtering performance of SPP, PPP and DGPS with different (σ 2 ) values, as shown in Figure 7, indicates that an exaggerated GNSS measurement noise covariance will considerably decrease positioning accuracy. On the other hand, a bold GNSS measurement noise variance (shown in SPP positioning errors in Figure 7) is acceptable for positioning accuracy, yet it damages the credibility of the positioning results, because the variance was overconfident. Therefore, it is important to adopt proper measurement noise variance values.
If the filter is able to estimate GNSS measurement noise variance itself, apparently the filter has the potential of performing better with respect to the accuracy of positioning. The proposed filter is the one estimating GNSS measurement noise variance based on extra DTM information. In order to implement GNSS measurement error sampling, a sampling period of 30 s was adopted, due to the fact that the stability of a small sample size will be affected by random gross error; on the contrary, a large sample size means more sampling time and less real time performance. The positioning results of proposed adaptive algorithm are revealed in Figure 8. In these tests, the algorithm set a prior GNSS measurement noise variance σ 2 = 75, which means the sole direction measurement noise standard deviation was five. The number of particle was 20,000, and the filtering period was 1 s, while the INS output frequency was 10 Hz. In order to further take advantage of DTM, the GNSS down-direction offset was estimated along with every sampling period, the GNSS velocity noise variance was set as one tenth of the position noise variance. Figure 8 shows that the filtering results converged after a few periods of sampling. The DGPS adaptive filter performed best when the test train was static in the middle of the test period. Meanwhile, the SPP and PPP adaptive filters showed some turbulence. Figure 9 describes the estimated GNSS measurement noise variance during adaptive filtering. the PPP and DGPS filters provided more stable estimation than the SPP filter. It is widely known that GNSS down-direction error is usually larger than that of other directions, which makes SPP positioning error much larger than 1 m at most times; however, the SPP measurement noise variance shown in Figure 9 is even less than one, due to the GNSS down-direction offset being estimated and finely eliminated along with every sampling period. This resulted in a remarkable improvement in accuracy in SPP. All three filters nonetheless provided more accurate and stable positioning results.
Some details are shown in Tables 1-3. Adaptive EPF after convergence in the following tables denotes adaptive GNSS/INS EPF filtering results after three periods of sampling. The results in this section clearly illustrate that the adaptive EPF algorithm took advantage of extra information provided by DTM. The position error mean reduced 58.6%, 11.5% and 47.6% in the SPP-, PPP-and DGPS-based filters in comparison to nominal EPF results, respectively. More importantly, the proposed algorithm obtained 62.4% and 14.9% improvements in position error mean in contrast to stand-alone SPP and PPP positioning solutions, respectively, considering the fact that usually the fusion results were approximate to or worse than the GNSS measurements.

Conclusions
This paper presents the positioning performance of a DTM-aided adaptive extended particle filter in relation to Standard Point Positioning (SPP), Precise Point Positioning (PPP) and Differential GPS (DGPS) techniques. The proposed algorithm was designed to estimate GNSS measurement noise variance adaptively based on added digital track map information, which makes the filter able to access the actual GNSS measurement noise variance. The GNSS down-direction offset is also estimated along with every sampling period by making full use of DTM information. Some on-site experiments were conducted in a nearly real railway environment. The results revealed in this paper demonstrate that the proposed adaptive algorithm gas the ability to decrease positioning errors in the fusion process, due to the introduction of DTM. The best performance improvement is observed in SPP/INS EPF. The adaptive filters even perform better than the SPP and PPP positioning due to the variance estimation of the adaptive GNSS measurement noise and the correction of the GNSS down-direction offsets. This paper proves the key role of DTM in positioning. The track map is a requisite for navigation and localization in railways. A better way of taking advantage of it results in more accurate positioning solution. In order to improve robustness, a failure detection process may be added in the proposed algorithm in future research. It can be a Kalman Filter-based scheme, since the proposed algorithm adopts EKF to estimate the unknown state vector and update the measurement covariance matrix. Alternatively, taking advantage of the particle filter, the state vector noise can be modelled as a non-Gaussian distribution.