Tightly Coupled GNSS/INS Integration with Robust Sequential Kalman Filter for Accurate Vehicular Navigation

With the development of multi-constellation multi-frequency Global Navigation Satellite Systems (GNSS), more and more observations are available for tightly coupled GNSS/Inertial Navigation System (INS) integration. Concerning the accuracy, robustness, and computational burden issues in the integration, we proposed a robust and computationally efficient implementation. The new tight integration model uses pseudorange, Doppler and carrier phase simultaneously, to achieve the maximum possible navigation accuracy for a single receiver. The resultant high-dimensional observation vector is then processed by a sequential Kalman Filter (KF) to improve the computational efficiency in the measurement update step. Based on the innovation of the sequential KF, a robust estimation method with Gaussian test is further devised to detect and adapt the faults in individual GNSS channels. Two field vehicular tests are conducted to evaluate the performance improvements of the proposed method, compared with loose coupling and conventional tight coupling. Test results in favorable environments indicate that the proposed method can significantly improve the velocity and attitude accuracy by 69.42% and 47.16% over loose coupling and by 64.75% and 30.88% over conventional tight coupling, respectively. Moreover, the computational efficiency is also improved by about 53.09% for the proposed method, compared with batch KF processing. In GNSS challenging environments, the proposed method also shows superiority in terms of velocity and attitude accuracy, and better bridging capability during the GNSS partial or complete outages. These results demonstrate that the proposed method is able to provide a more robust and accurate solution in real-time vehicular navigation.


Introduction
Accurate and reliable navigation information is a fundamental basis of many vehicular applications such as autonomous driving, location based services, and traffic management. With global coverage, all-weather capability, and good long-term accuracy, Global Navigation Satellite Systems (GNSS), such as the U.S. Global Positioning System (GPS), the Chinese BeiDou navigation satellite system (BDS), Russian GLObal NAvigation Satellite System (GLONASS), and European Galileo, are widely utilized in current vehicle navigation systems. However, using GNSS alone has certain limitations. First, the satellite visibility is completely or partially obscured in degraded environments such as urban canyons, tunnels, and buildings. Second, the GNSS signal is vulnerable to interference such as innovation. In TC, it cannot effectively identify which measurement channels contain faults and the high accuracy of good observations will be sacrificed.
With the advent of multi-constellation multi-frequency GNSS, multi-type observations such as pseudorange, Doppler, and carrier phase of multiple satellites are available for measurement update in TC of GNSS/INS integration. The computational burden when processing observation vectors of high dimension must be considered, especially for an embedded CPU. In conventional KF, the observation vector is processed in one single measurement update, which is time consuming due to the inversion of high-dimensional matrix. Alternatively, the high-dimensional observations can be decomposed into multiple low-dimensional or scaler observations and then the measurement update is performed sequentially [27]. The current researches about TC mainly focus on the modeling and robustness enhancement of GNSS/INS integration. Few papers are devoted to analyzing the computation cost of integration algorithms.
Considering the accuracy, robustness, and computational efficiency problems, we propose a robust and efficient implementation of tightly coupled GNSS/INS integration. All type observations, including pseudorange, Doppler, and carrier phase, are fused with INS to achieve the maximum possible navigation accuracy for a single receiver (TC-PDC). To improve the computational efficiency, sequential KF is employed to process the high-dimensional observations. Based on the innovation of sequential KF, a robust estimation method with Gaussian test is designed to detect and adapt the faults in individual channels. The performance of the proposed method, an terms of accuracy, robustness and computational efficiency, is analyzed by two vehicular field tests. Our contribution differs from previous work in the following aspects. (1) A complete performance comparison among LC, TC-PD, and TC-PDC is conducted to show the improvements of position, velocity, and attitude accuracy brought by the TDCP measurement. (2) A sequential robust estimation method is devised to utilize the faulty and good observations rationally. (3) The computation times are analyzed to reveal the superiority of sequential over batch mode. This paper is organized as follows. Section 2 describes the GNSS observation equations and the error mitigation strategies. Section 3 establishes the mathematical model for TC GNSS/INS integration. The corresponding robust sequential KF is presented in Section 4. The experimental results and analysis are given in Section 5. Finally, Section 6 concludes the whole paper.

GNSS Observations
The generic basic observation equations for pseudorange, Doppler, and carrier phase are given as follows P s r = ρ s r + c(dt r − dt s ) + I s r + T s r + ε s P,r where P s r , D s r , and ϕ s r are respectively pseudorange, Doppler, and carrier phase observations, ρ s r is the geometric range between a receiver-satellite pair, dt r and dt s are the receiver and satellite clock error, I s r and T s r are the ionospheric and tropospheric delay, N s r is the integer ambiguity, c is the speed of light, λ is carrier phase wavelength, and ε s P,r , ε s D,r , and ε s Φ,r represent the code, Doppler, and carrier phase noise, respectively.
To mitigate the nuisance errors and delays above, difference and combination techniques are adopted. We apply between-satellite single difference to eliminate receiver clock error. For carrier phase, time difference is also added to remove the ambiguity parameter. For pseudorange observation, the ionospheric delay is compensated using the dual-frequency ionosphere-free combination, and the tropospheric delay is mostly corrected by the Saastamoinen model [28]. Considering atmospheric then the compensated equations for Equation (1) read with I ij IF and T ij Sa the calculated ionospheric and tropospheric delays by the above methods.

Tightly-Coupled GNSS/INS Integration Model
In this paper, GNSS/INS integration is implemented through indirect KF with feedback correction. The linear INS error model augmenting sensor error stochastic process constitutes the system model. The integration KF directly fuses GNSS raw observations and INS-derived counterparts to estimates the error states, which are fed back into the INS to correct the navigation error and sensor biases. In the following, the dynamic model and measurement model for the integration are developed.

Dynamic Model
The INS psi-angle error model reads where superscripts n and b denote the navigation frame (n-frame) and the body frame (b-frame), respectively; δr n , δv n , and ψ are the position, velocity, and attitude error vectors, respectively; ω n ie is the earth rotation rate with respect to the inertial frame (i-frame); ω n en is the transport rate representing the turn rate of the n-frame with respect to the earth-centered earth-fixed frame (e-frame); f b is the specific force measured by the accelerometers; δg n is the gravity error which can be expressed as the function of the INS position, velocity, and misalignment angle; C n b is the direction cosine matrix (DCM) from b-frame to n-frame; ∇ b and ε b are the bias errors for accelerometers and gyros, modeled as first-order Gauss-Markov processes with T a , T g are the respective correlation times, and ω a , ω g are the respective driven noise for accelerometers and gyros. These parameters can be obtained using Allan variance analysis of inertial sensors [29]. Equations (4) and (5) constitute the dynamic model, which can be written as a compact form .
where F is the dynamic matrix, G is the noise distribution matrix, and x is the state vector, including 9 navigation error states and 6 sensor bias states Sensors 2020, 20, 561 5 of 19

Measurement Model
In order to get the measurement model, the nonlinear observation Equation (3) can be linearized with respect to the unknown receiver positon and velocity ρ ij r,I , and ∆ t ρ ij r,I are the corresponding predicted observations using INS states estimates, e ij is the single-difference line-of-sight vector, C e n is the DCM from n-frame to e-frame, δr n G and δv n G are the position and velocity error vectors of GNSS antenna phase center.
The position and velocity of GNSS antenna phase center are related to those of IMU center as follows [26] r n where r n G and v n G represent the position and velocity of GNSS antenna phase center, r n and v n are the position and velocity of IMU center, ω n in = ω n ie + ω n en is the rotation rate of n-frame with respect to the i-frame, ω b ib is the angular rate of b-frame with respect to i-frame measured by the gyros, l b is the lever arm, and (·×) is the skew-symmetric matrix. The corresponding position and velocity error vectors of GNSS and INS can be further derived as Defining the following matrices and substituting Equations (10) and (11) into Equation (8) yields The carrier phase equation in Equation (8) is related to both the current and past position errors, whichviolates the assumption of a normal KF. In this deduction, we perform backward time propagation to reformulate the past state as where Φ k,k−1 is the state transition matrix from epoch k − 1 to k. Using Equation (13), the linearized phase equation can be expressed as Based on Equations (12) and (14), the measurement model for tightly-coupled GNSS/INS integration is given as Sensors 2020, 20, 561 H is the measurement matrix given by and ε = is the observation noise vector.

Robust Sequential KF
With the development of multi-constellation multi-frequency GNSS, at each measurement update epoch, an increasing number of pseudorange, Doppler, and carrier phase observations are available. When implementing tightly coupled GNSS/INS integration, the computational efficiency needs to be considered. What is also important is that prior to processing any type of observation, a health check should be performed to detection and handle faults.

Sequential KF
For the Kalman filtering processing of the GNSS observations, two options exist. One is batch processing. All the observations form a vector which is then processed in one single measurement update. The other option is sequential processing which performs a scalar measurement update for each observation. In batch processing, a matrix with the number of rows equal to the number of available observations has to be inverted in the calculation of the Kalman gain matrix. Whereas in sequential processing, the matrix inversion reduces to a simple division, which not only improves the computational efficiency but also guarantees the stability of numerical calculation in the case of massive observations. Moreover, the sequential processing enables detection of faults in individual GNSS channels, which will be shown in the next subsection.
The time update step is the same for both sequential mode and batch processinĝ Sensors 2020, 20, 561 7 of 19 with the initial state and its covariance matrix given bŷ

Robust Estimation Based on Innovation Test
It is well-known that the KF innovation is Gaussian distributed with zero mean. The GNSS faults would violate this property and thus degenerate the filter. In the worst case, the filter may diverge. In GNSS/INS integration, it is very important to resist this adverse influence of GNSS faults.
According to Equation (20), an the i-th measurement update at epoch k, The KF innovation v i k and the corresponding variance wherex i−1 k is the estimated state after the (i − 1)th measurement update. In the absence of faults, the mean of innovation will be zero. Otherwise, the mean will be shifted. Therefore, the hypothesis test can be conducted to detect the faults with null hypothesis H 0 and alternative hypothesis H 1 chosen as where δ is the fault. The test statistic is defined as t obeys standard normal distribution and Equation (23) changes to The threshold can be determined using the distribution probability function to meet the required probability of false alert. A fault will be detected if the test statistic and the threshold meet For a given probability of false alert α, the threshold T d can be given by the 1 − α 2 quantile n 1−α/2 of standard normal distribution function, i.e., T d = n 1−α/2 , which means that the decision is made at the risk of α. As shown in Figure 1, the test threshold controls the probabilities of false alert and miss detection, and there is a tradeoff between them. An increase in the threshold implies a lower false alarm rate and higher miss detection rate, and vice versa. Generally, the required probability of false alert is chosen by rule of thumb. In this paper, the value of α is set as 0.001.
Once a fault is detected, then the following scaling factor is calculated to inflate the variance κ 2 is the scaling factor which indicates the discrepancy between the actual variance and its theoretical value. By inflating the variance, the detected faulty measurement can be downweighted in the update process of KF. The KF gain in Equation (20) can then be adaptively adjusted to resist the adverse effects of GNSS faults. After processing the previous (i − 2)th observations,x i−1 k is more accurate thanx i−2 k , and of course more accurate than the predicted valuex k/k−1 , i.e., P i−1 k ≤ P i−2 k · · · ≤ P k/k−1 , leading to a larger δ of Equation (25). As shown in Figure 1, a larger δ makes less intersect areas, so smaller miss detection probability P MD could be achieved. Consequently, it is more probable to correctly detect the fault. Hence, better reference information (more accuratex i k ) implies a better capability of detecting faults. However, an robust KF with batch processing [26], only the predictedx k/k−1 and P k/k−1 are available. It can then be concluded that robust sequential KF could not only detect faults in individual channels, but also possess better detection capability. 2 κ is the scaling factor which indicates the discrepancy between the actual variance and its theoretical value. By inflating the variance, the detected faulty measurement can be downweighted in the update process of KF. The KF gain in Equation (20) can then be adaptively adjusted to resist the adverse effects of GNSS faults. After processing the previous ( 2 i − )th observations, 1i k x − is more accurate than 2 i k x − , and of course more accurate than the predicted value / 1k k x − , i.e., , leading to a larger δ ′ of Equation Error! Reference source not found.. As shown in Figure 1, a larger δ ′ makes less intersect areas, so smaller miss detection probability M D P could be achieved. Consequently, it is more probable to correctly detect the fault. Hence, better reference information (more accurate ˆi k x ) implies a better capability of detecting faults. However, in robust KF with batch processing [26], only the predicted / 1k k x − and / 1 k k P − are available. It can then be concluded that robust sequential KF could not only detect faults in individual channels, but also possess better detection capability.  Figure 2 shows the block diagram of the proposed tightly-coupled GNSS/INS integration algorithm. In the figure, the solid and dash arrows are the INS-related high-rate and GNSS-related low-rate information flows, respectively. INS mechanization calculates the high-rate navigation result (position, velocity, and attitude) with the compensated raw INS data. In parallel, the GNSS receiver provides low-rate pseudorange, Doppler, carrier phase, and ephemeris data. Then systematic error sources are corrected using combination and difference techniques, along with some empirical models. The corrected GNSS observations and the INS-predicted counterparts form the OMC measurements. The robust sequential KF uses the OMC measurements to estimate the error

Test and Results
In this section, two vehicular test results are presented. The first one compares the performance of different integration and update processing strategies in terms of accuracy and efficiency in favorable environments. The second test focuses on testing the robustness of the algorithm in harsh environments. For both tests, MEMS-and high-grade IMUs were used. The data from MEMS-grade IMU is processed using the proposed algorithm. The data collected by the base station and highgrade IMU is post processed using RTK/INS integration with backward smoothing by the GINS software of Wuhan MP Space Time Technology Company, which is used as a reference. For better observability of the system state, the vehicle run along trajectories on which there are numerous motion types (acceleration, deceleration, and turn).

Test and Results
In this section, two vehicular test results are presented. The first one compares the performance of different integration and update processing strategies in terms of accuracy and efficiency in favorable environments. The second test focuses on testing the robustness of the algorithm in harsh environments. For both tests, MEMS-and high-grade IMUs were used. The data from MEMS-grade IMU is processed using the proposed algorithm. The data collected by the base station and high-grade IMU is post processed using RTK/INS integration with backward smoothing by the GINS software of Wuhan MP Space Time Technology Company, which is used as a reference. For better observability of the system state, the vehicle run along trajectories on which there are numerous motion types (acceleration, deceleration, and turn).

Test #1
The test was performed in March 2015 in a suburb of Wuhan, China, as shown in Figure 3. Two sets of navigation systems were mounted on the roof of the test vehicle. The first is a prototype GNSS/INS system which integrates low-cost STIM300 MEMS with Novatel OEMV-3 receiver. The raw data from the prototype system was processed and analyzed with the proposed algorithm. The sampling rates were set 125 Hz and 1 Hz for MEMS and GNSS receiver, respectively. The second is a reference system (POS830, manufactured by Wuhan MP Space Time Technology Company, Wuhan, China). A temporary base station was set up nearby whose coordinate was determined through PPP processing of 2-h static GPS/GLONASS/BDS data. The resultant solutions act as the reference to evaluate the performance of the proposed algorithm when applied to MEMS-based INS. Figure 4 shows the test setup. Tables 1 and 2 list the performance parameters of the two systems.
In this section, two vehicular test results are presented. The first one compares the performance of different integration and update processing strategies in terms of accuracy and efficiency in favorable environments. The second test focuses on testing the robustness of the algorithm in harsh environments. For both tests, MEMS-and high-grade IMUs were used. The data from MEMS-grade IMU is processed using the proposed algorithm. The data collected by the base station and highgrade IMU is post processed using RTK/INS integration with backward smoothing by the GINS software of Wuhan MP Space Time Technology Company, which is used as a reference. For better observability of the system state, the vehicle run along trajectories on which there are numerous motion types (acceleration, deceleration, and turn).

Test #1
The test was performed in March 2015 in a suburb of Wuhan, China, as shown in Figure 3. Two sets of navigation systems were mounted on the roof of the test vehicle. The first is a prototype GNSS/INS system which integrates low-cost STIM300 MEMS with Novatel OEMV-3 receiver. The raw data from the prototype system was processed and analyzed with the proposed algorithm. The sampling rates were set 125 Hz and 1 Hz for MEMS and GNSS receiver, respectively. The second is a reference system (POS830, manufactured by Wuhan MP Space Time Technology Company, Wuhan, China). A temporary base station was set up nearby whose coordinate was determined through PPP processing of 2-h static GPS/GLONASS/BDS data. The resultant solutions act as the reference to evaluate the performance of the proposed algorithm when applied to MEMS-based INS. Figure 4 shows the test setup. Tables 1 and 2 list the performance parameters of the two systems.        The GNSS receiver was set to track GPS (L1 and L2) and BDS (B1, B2, and B3) dual-constellation five-frequency signals. Figure 5 shows the sky plot and the satellite visibility in terms of satellite number and positional dilution of precision (PDOP) (15 • elevation mask). It can be observed that the geometry is favorable, and the average number of visible satellites and PDOP are 17.26 and 1.46, respectively. This indicates that multi-GNSS combinations can raise the number of available satellites and strength the station-satellite geometry significantly. To clearly show the advantages of the proposed algorithm, we processed the data in three integration schemes: loose coupling (LC), tight coupling based on pseudorange and Doppler (TC-PD), and tight coupling based on pseudorange, Doppler, and carrier phase (TC-PDC). In addition, batch and sequential measurement update strategies were all implemented. We first compare the accuracy performance of the three schemes. The enhancement of the robust estimation in the presence of GNSS faults, especially during complete GNSS outages, is analyzed in the sequel. Finally, the computational speed of the two measurement update strategies is compared to reveal the improvement of computational efficiency brought by the sequential KF.  To clearly show the advantages of the proposed algorithm, we processed the data in three integration schemes: loose coupling (LC), tight coupling based on pseudorange and Doppler (TC-PD), and tight coupling based on pseudorange, Doppler, and carrier phase (TC-PDC). In addition, batch and sequential measurement update strategies were all implemented. We first compare the accuracy performance of the three schemes. The enhancement of the robust estimation in the presence of GNSS faults, especially during complete GNSS outages, is analyzed in the sequel. Finally, the computational speed of the two measurement update strategies is compared to reveal the improvement of computational efficiency brought by the sequential KF. Figure 6 plots the position, velocity and attitude errors of the three integration schemes. Table 3 summarizes the corresponding root-mean-square errors (RMSE). Compared with LC, the RMSE values of TC-PD decrease from 3.7490 m, 0.0601 m/s, and 0.1463 deg to 3.5867 m, 0.0538 m/s, and 0.1086 deg with improvements of 4.33%, 10.48%, and 25.77% in the position, velocity, and attitude. As expected, TC outperforms LC. For TC-PDC, the RMSE values of velocity and attitude further decrease to 0.0272 m/s and 0.0795 deg, with significant improvements of 54.74% and 45.66%. However, the position accuracy becomes a bit worse with a degradation of 10.93%. From Figure 6, the position errors of TC-PDC abruptly jumps around 1555 s (marked with a black rectangle), making the succeeding position errors increase. The velocity and attitude are also observed gross errors around the same time instant. This abnormal errors could be caused by the cycle slip of carrier phase, as they are absent for LC and TC-PD. These outlying navigation results will cause unacceptable degradations in the control performance, especially for some high-speed safety-related applications, thus have to be carefully addressed.

Performance of Robust Estimation
For the same data set, robust estimation is applied to resist the faults in GNSS observations. Figure 7 illustrates the performance of robust KF for the three integration schemes, and Table 4 summarizes the corresponding RMSE. Comparing Figure 6 with Figure 7, the jump errors of TC-PDC disappear. The proposed robust estimation method eliminates the abnormal navigation errors. The position error of TC-PDC is more stable than the two counterparts, which leads to better accuracy of position increments. Comparing Table 3 with Table 4, with robust KF, the position and velocity accuracy of TC-PDC are significantly improved by 19.65% (from 4.1588 m to 3.3416 m) and 34.56% (from 0.0272 m/s to 0.0178 m/s); For TC-PD, the robust KF slightly improves the position and velocity accuracy by 1.42% (from 3.5867 m to 3.5359 m) and 6.13%(from 0.0538 m/s to 0.0505 m/s); For LC, only velocity accuracy achieves a slight improvement of 3.16% (from 0.0611 m/s to 0.0593 m/s). These results demonstrate the superiority of robust estimation in detecting and resisting the faults. For the three integration schemes with robust KF, the RMSE of LC decrease from 3.7526 m, 0.0582 m/s, and 0.1478 deg to 3.3416 m, 0.0178 m/s, and 0.0781 deg of TC-PDC with improvements of 10.95%, 69.42% and 47.16%. Additionally, robust TC-PDC is also superior to robust TC-PD, which improves the navigation accuracy by 5.50%, 64.75% and 30.88% for position, velocity and attitude, respectively. Obviously, the introduction of time-differenced carrier phase remarkably improved the velocity accuracy. The overall attitude accuracy also benefits.
(from 0.0272 m/s to 0.0178 m/s); For TC-PD, the robust KF slightly improves the position and velocity accuracy by 1.42% (from 3.5867 m to 3.5359 m) and 6.13%(from 0.0538 m/s to 0.0505 m/s); For LC, only velocity accuracy achieves a slight improvement of 3.16% (from 0.0611 m/s to 0.0593 m/s). These results demonstrate the superiority of robust estimation in detecting and resisting the faults. For the three integration schemes with robust KF, the RMSE of LC decrease from 3.7526 m, 0.0582 m/s, and 0.1478 deg to 3.3416 m, 0.0178 m/s, and 0.0781 deg of TC-PDC with improvements of 10.95%, 69.42% and 47.16%. Additionally, robust TC-PDC is also superior to robust TC-PD, which improves the navigation accuracy by 5.50%, 64.75% and 30.88% for position, velocity and attitude, respectively. Obviously, the introduction of time-differenced carrier phase remarkably improved the velocity accuracy. The overall attitude accuracy also benefits.   According to Figure 5, the preceding results are obtained in a favorable observation environment (The minimal number of visible satellites are greater than 5). To assess the performance of the proposed algorithm in GNSS denied and degraded areas (urban canyons or open pit mines), several complete GNSS outages, each lasting 60 s, are simulated after the navigation filter converges. The simulated outages are implemented by masking all the visible satellites artificially. During these outages, the navigation solution is solely dependent on inertial navigation, i.e., no closed-loop correction during the 60 s. Figure 8 compares the navigation errors among the three integration schemes. TC-PDC achieves smallest navigation errors in most cases, followed by TC-PD and LC. The corresponding RMSE and maximum errors are listed in Table  correction during the 60 s. Figure 8 compares the navigation errors among the three integration schemes. TC-PDC achieves smallest navigation errors in most cases, followed by TC-PD and LC. The corresponding RMSE and maximum errors are listed in     The above results demonstrate that TC-PDC outperforms LC and TC-PD in terms of navigation performance. For real-time implementation of integration algorithm, an important concern is the computational cost. It is necessary to compare the computation times of different processing strategies for the measurement update of the three integration schemes. For comparison purpose, all the measurements are processed in a normal way, i.e., no fault detection and adaption, which corresponds to Section 5.1.2. All the algorithms were developed in ANSI C language and all computations were performed on an Intel Core i5 3.30 GHz PC with 8 GB memory running Windows 7. Figure 9 compares the computation times of measurement update for the three integration schemes. The top and bottom panels correspond to the computation times of sequential and batch modes, respectively. As can be seen in the figure, TC-PDC is the slowest, followed by TC-PD. LC has extremely low computational times (lower than 1 ms). The sequential mode impressively improves the computational efficiency for TC-PDC, whereas this is not the case for LC and TC-PD. Table 6 reports the mean value of computation times. Compared with batch processing, the sequential processing significantly improves the computational efficiency for TC-PDC by 53.10% (from 7.098 ms to 3.329 ms). For TC-PD, there is a slight improvement, just 1.87% (from 1.979 ms to 1.942 ms). However, the mean computation time for LC adversely increases a bit.   Figure 10 plots the mean computation times with the increase of number of observations in TC-PD and TC-PDC for sequential and batch processing. As the number of observations increases, the computation times of batch and sequential modes increase in polynomial and linear ways, respectively. When the number of observations are more than about 54, the sequential mode is faster than batch mode and this superiority is more significant for a large number of observations. The main reason behind is that the computational cost of matrix inversion in batch mode proportional to the third order of dimension. From Equation Error! Reference source not found., it is easy to conclude that the computation times of measurement update in sequential mode proportional to the number of observations to be processed. Therefore, it is suggested that for multi-constellation multi-frequency GNSS/INS tightly coupled integration, sequential measurement update should be adopted to   Figure 10 plots the mean computation times with the increase of number of observations in TC-PD and TC-PDC for sequential and batch processing. As the number of observations increases, the computation times of batch and sequential modes increase in polynomial and linear ways, respectively. When the number of observations are more than about 54, the sequential mode is faster than batch mode and this superiority is more significant for a large number of observations. The main reason behind is that the computational cost of matrix inversion in batch mode proportional to the third order of dimension. From Equation (20), it is easy to conclude that the computation times of measurement update in sequential mode proportional to the number of observations to be processed. Therefore, it is suggested that for multi-constellation multi-frequency GNSS/INS tightly coupled integration, sequential measurement update should be adopted to improve the computational efficiency.  Figure 10 plots the mean computation times with the increase of number of observations in TC-PD and TC-PDC for sequential and batch processing. As the number of observations increases, the computation times of batch and sequential modes increase in polynomial and linear ways, respectively. When the number of observations are more than about 54, the sequential mode is faster than batch mode and this superiority is more significant for a large number of observations. The main reason behind is that the computational cost of matrix inversion in batch mode proportional to the third order of dimension. From Equation Error! Reference source not found., it is easy to conclude that the computation times of measurement update in sequential mode proportional to the number of observations to be processed. Therefore, it is suggested that for multi-constellation multi-frequency GNSS/INS tightly coupled integration, sequential measurement update should be adopted to improve the computational efficiency.

Test #2
The second test was performed in a city urban environment in Wuhan, China, on 11 September 2018, as shown in Figure 11. The trajectory was generated using RTKLIB program package (http://www.rtklib. com), where green and yellow dots stand for fixed and float solutions, respectively. The vehicle drove four typical GNSS changeling environments successively, including

Test #2
The second test was performed in a city urban environment in Wuhan, China, on 11 September 2018, as shown in Figure 11. The trajectory was generated using RTKLIB program package (http://www.rtklib.com), where green and yellow dots stand for fixed and float solutions, respectively. The vehicle drove four typical GNSS changeling environments successively, including The test equipment is shown in Figure 12. The GNSS/MEMS-SINS Prototype consists of STIM 300 MEMS and ComNav K708 receiver. The sampling rates of INS measurements and GNSS observations were 125 Hz and 10 Hz, respectively. The collected data was processed and analyzed in the sequel. POS620 from Wuhan MP Space Time Technology Company was used for generating the reference. The corresponding main performance parameters of POS620 are shown in Table 7. The test equipment is shown in Figure 12. The GNSS/MEMS-SINS Prototype consists of STIM 300 MEMS and ComNav K708 receiver. The sampling rates of INS measurements and GNSS observations were 125 Hz and 10 Hz, respectively. The collected data was processed and analyzed in the sequel. POS620 from Wuhan MP Space Time Technology Company was used for generating the reference. The corresponding main performance parameters of POS620 are shown in Table 7. The test equipment is shown in Figure 12. The GNSS/MEMS-SINS Prototype consists of STIM 300 MEMS and ComNav K708 receiver. The sampling rates of INS measurements and GNSS observations were 125 Hz and 10 Hz, respectively. The collected data was processed and analyzed in the sequel. POS620 from Wuhan MP Space Time Technology Company was used for generating the reference. The corresponding main performance parameters of POS620 are shown in Table 7.    Figure 13 shows the sky plot distribution of available dual-constellation (GPS + BDS) satellites and the corresponding satellite number and PDOP during the test. Compared with test #1, the satellite number varies frequently and the geometry is poor (less visible satellites and large PDOP value) during the four typical environments. The minimal satellite number is less than 4. Especially in the third environment (Tunnel), a complete satellite outage is observed (about 36 s).   Figure 13 shows the sky plot distribution of available dual-constellation (GPS + BDS) satellites and the corresponding satellite number and PDOP during the test. Compared with test #1, the satellite number varies frequently and the geometry is poor (less visible satellites and large PDOP value) during the four typical environments. The minimal satellite number is less than 4. Especially in the third environment (Tunnel), a complete satellite outage is observed (about 36 s).
As is well known, LC is inferior to TC in GNSS challenging areas where the available satellites is less than 4. Test #1 also demonstrates the superiority of TC over LC. Therefore, we only compared the results of two TC integration schemes: TC-PD and TC-PDC and it reveals the importance of robust estimation in TC, especially in GNSS challenging environments. As is well known, LC is inferior to TC in GNSS challenging areas where the available satellites is less than 4. Test #1 also demonstrates the superiority of TC over LC. Therefore, we only compared the results of two TC integration schemes: TC-PD and TC-PDC and it reveals the importance of robust estimation in TC, especially in GNSS challenging environments.

Performance in GNSS Challenging Environments
For comparison, the TC solutions without robust estimation were also calculated as plotted in Figure 14. The corresponding RMSE and maximum errors are listed in Table 8. As expected, robust estimation significantly improves the accuracy of TC-PD and TC-PDC, especially in GNSS challenging environments (gird shed and tunnel). Take the position as example, the maximum errors decrease from 27.7496 m to 10.0729 m for TC-PD, and 15.4275 m to 8.1609 m for TC-PDC. Slight improvements are achieved in the foliage scenario as the observation environments are more moderate, as indicated by the satellite number and PDOP. Concerning the error statistics, the accuracy improvements obtained from robust estimation are 11.19%, 34.89%, and 14.60% for TC-PD, and 5.67%, 37.72%, and 29.04% for TC-PDC, respectively, an the position, velocity, and attitude. In addition, it can be observed that robust TC-PDC is superior to robust TC-PD rival, which improves the navigation accuracy by 3.49% for position, 29.53% for velocity, and 1.47% for attitude. The results demonstrate that the robust estimation can significantly promote the TC integration accuracy in GNSS challenging areas, especially for the velocity and attitude components.

Conclusions
With the rise of multi-constellation multi-frequency GNSS, tightly coupled integration of GNSS/INS benefits a lot in terms of accuracy, availability, and reliability. However, some nuisance issues also arise such as high computation burden and frequently encountered GNSS observation faults. In this research, we proposed a tightly coupled GNSS/INS integration algorithm using robust sequential KF for accurate vehicular navigation. Apart from pseudorange and Doppler used in traditional tight coupling, the time differenced carrier phase measurement is also incorporated to implement GNSS/INS data fusion. Considering the high dimension of observations, the measurement update step in KF is performed in a sequential way to improve the computational efficiency. Based on this sequential KF innovation, a robust estimation method is further developed to detect and resist the faults in individual channels. By doing so, the good observations can be kept

Conclusions
With the rise of multi-constellation multi-frequency GNSS, tightly coupled integration of GNSS/INS benefits a lot in terms of accuracy, availability, and reliability. However, some nuisance issues also arise such as high computation burden and frequently encountered GNSS observation faults. In this research, we proposed a tightly coupled GNSS/INS integration algorithm using robust sequential KF for accurate vehicular navigation. Apart from pseudorange and Doppler used in traditional tight coupling, the time differenced carrier phase measurement is also incorporated to implement GNSS/INS data fusion. Considering the high dimension of observations, the measurement update step in KF is performed in a sequential way to improve the computational efficiency. Based on this sequential KF innovation, a robust estimation method is further developed to detect and resist the faults in individual channels. By doing so, the good observations can be kept as much as possible. Test results of suburban and urban field experiments are presented and performance comparison with traditional LC and TC-PD is made to validate the effectiveness and superiority of the proposed algorithm. Based on the results and analysis above, we can draw the following conclusions: (1) Compared with TC-PD and LC, TC-PDC can significantly improves the velocity and attitude accuracy of integrated solutions, and further smooths the positioning error, which shows the superiority of the introduction of high-precision time differenced carrier phase observations. (2) The innovation-based robust estimation method could effectively detect and resist the GNSS faults in individual channels, which guarantees the reliability of state estimation in GNSS challenging environments. With high-precision time-differenced carrier phase and robust estimation method, TC-PDC also shows better navigation accuracy even during long GNSS outages. (3) For sequential KF, the computation cost will increase linearly with the increasing of the number of observations, while for batch KF, it is approximately proportional to the third order of the number of observations. The computational efficiency can be improved to a great extent if there are a large number of observations. Therefore, when implementing multi-constellation multi-frequency multi-type-observation tightly couple GNSS/INS integration, it is advised to adopt sequential KF to improve the computational efficiency.
In the future, the algorithm will be implemented on an embedded system consisting of low-cost MEMS and receiver to provide real-time navigation data, which plays a key role in vehicular applications. In addition, the integration with multiple auxiliary sensors such as odometer, radars, digital maps, cameras, etc., will also be considered as future work.