Introduction

Intelligent Transport Systems (ITS), which utilize communication, navigation and computing technologies to increase traffic capacity, enhance safety and reduce car emissions, are critical for long-term sustainability of the operation of transport systems. There are many ITS applications, including road navigation, road user charging, and advanced driving assistant systems. The positioning and navigation information, including positioning, velocity and attitude, are fundamental for these ITS applications. As one of the main navigation technologies, Global Navigation Satellite Systems (GNSS) such as the Global Positioning System (GPS), which are able to provide continuous spatial and temporal information of the vehicle state, are already widely used in ITS applications. Although most of the ITS applications can be fulfilled by current meter level positioning accuracy, for some mission-critical applications such as advanced driving assistant system and future autonomous driving, decimeter level positioning accuracy with high reliability is essential.

The main raw GNSS measurements are the pseudorange and carrier phase. Potentially, the pseudorange measurement can be used to obtain a positioning accuracy at the meter level, while carrier phase measurements can be used to obtain decimeter level accuracy in dynamic mode. However in urban areas, the accuracy and reliability of GNSS-based vehicle navigation systems are degraded due to signal blockage and attenuation, mainly due to signal reflection resulting in multipath and non-line of sight effects (Duong et al. 2019; Sun et al. 2021a, 2022). It is, therefore important to integrate GNSS with the Inertial Navigation System (INS) to improve the navigation performance of the vehicle (Chiang et al. 2019; Shin 2002; Zegarra and Farcy 2012; Sun et al. 2021b). INS has short-term stability with high data rate, while GNSS exhibits long-term stability with a relatively low data rate. Hence, the complementary characteristics of GNSS and INS can be used in an integration scheme to improve positioning and navigation.

The two ways to improve the accuracy and reliability of GNSS/INS fusion or integration are hardware enhancement and fusion strategy. Using expensive sensors to output measurements with higher quality, such as anti-multipath antenna and strategic-grade INS, is a typical way to improve the fusion performance based on hardware enhancement (Tranquilla et al. 1994; Jiang and Groves 2012). Nevertheless, exorbitant cost limits their use. Hence, significant effort is dedicated to improving the fusion strategy. This typically involves the selection of sensor measurements and configuration of the fusion mechanism for different types of technologies, including low-cost sensors.

Extensive research has been dedicated to fusion algorithms. Nayak investigated GNSS/IMU integration schemes with the aid of multiple antennas to mitigate multipath on pseudorange to improve positioning accuracy in urban areas (Nayak 2000). Shin (2005) investigated several filtering-based fusion algorithms to improve integrated GNSS/IMU results. Abdel-Hamid’s (2005) research focused on enhancing navigation accuracy using fuzzy logic-based techniques. These pseudorange-based fusion algorithms are capable of meter-level accuracy. However, for some applications, such as autonomous driving, decimeter accuracy is required, necessitating the use of carrier phase measurements.

For the positioning methods using carrier phase measurements, such as precise point positioning (PPP) and conventional real-time kinematic, time-consuming initialization or correction information is required to determine integer ambiguities for dynamic applications (An et al. 2020; Du et al. 2021; Elsheikh et al. 2019; Maddern et al. 2020; Zhang et al. 2020). This limits the use of carrier phase measurements for ITS applications. A time-differenced carrier phase (TDCP) observable can be obtained by differencing between two successive carrier phase measurements when there is no cycle slip. TDCP has been widely used to obtain the change in position for adjacent epochs (Soon et al. 2008), velocity estimation (Serrano et al. 2004; Van Graas and Soloviev 2003; Wendel et al. 2006; Ding and Wang 2011) and heading and pitch estimations (Sun et al. 2020), without the difficulty of ambiguity resolution.

The merit of using the TDCP observations is to obtain high accuracy attitude and velocity estimations without solving for the integer ambiguity when there is no cycle slip. Therefore, TDCP is used to aid various GNSS/INS integrated navigation systems to enhance navigation performance. Wendel et al. (2004, 2006) proposed a GNSS/INS integration scheme with velocity information from TDCP. However, according to Han and Wang (2012) the lack of absolute position information could cause large position drift. In addition, Han and Wang designed a tightly coupled method with the integration of the TDCP, pseudoranges, and INS measurements to reduce the velocity, and attitude errors by at least one-half for a 45-min field navigation experiment with the 2.70 m 3D positioning accuracy achieved. Tang et al. (2007) used the observation equation of TDCP along with the Kalman–Schmidt reduced filter to execute the passive-mode Radio Determination Satellite Service (RDSS)/INS to improve the velocity estimation accuracy of ships. The max positioning errors of 20 m in the east and 50 m in the north are obtained. Ding et al. (2008) designed a two-step calibration scheme with delta position from TDCP, suppressing the divergence of INS error and meanwhile inhibiting the overall error growth of position and attitude by GPS pseudorange measurements. However, only velocity errors are analyzed and the issue of cycle slips is ignored in their algorithm. The existence of cycle slips could significantly impact the system performance, such as degrading the reliability and accuracy of navigation solutions. Travis (2010) used a delayed state Kalman filter to make the measurement model fit the standard form of the Kalman filter in TDCP/INS approach and detect cycle slips by double differencing. However, cycle slip repair was abandoned due to heavy computation load. Zhao (2017) introduced both the conventional and modified TDCP observations into GPS/IMU tightly coupled navigation under the assumption of no cycle slip, which is unrealistic for a moving vehicle, especially in an urban environment. Also, only positioning errors are analyzed, achieving an RMSE of 0.2 m in horizontal. Li et al. (2018) enhanced the tightly coupled navigation approach with a three-dimensional phase-derived position increment (PDPI) and improved computation efficiency with modeled noise characteristics. Nevertheless, there is no accuracy improvement appeared in the vertical position, roll and pitch. An extended Kalman filter (EKF) is used to integrate TDCP with INS, with the consideration of the noise correlation (Kim et al. 2019) and an INS propagation-aided cycle slip detection (Kim et al. 2020). Herein, although INS data was adapted, only positional error was analyzed, ignoring velocity and attitude.

The research above on integrated GNSS/INS systems with TDCP has two problems. One is carrier phase cycle slip, which cannot be predicted and avoided in dynamic applications in difficult environments such as cities. Thus, it is necessary to detect and repair cycle slips while meeting the requirements of computational effort associated with real-time applications. The other problem is that most researchers focus on augmentation and incorporation of velocity into GNSS/INS systems to improve positioning accuracy. However, integration of other potential estimations derived from TDCP into GNSS/INS systems is also crucial for vehicle navigation to provide more accurate navigation information. Furthermore, part of vehicle attitude estimation, i.e., pitch and yaw, can be provided by TDCP with relatively high accuracy within an integrated system.

We propose a new TDCP-aided GNSS/IMU integrated navigation algorithm in urban environments. The proposed algorithm is based on a two-stage filter scheme for state estimation with carrier phase cycle slip detection and repair. The proposed algorithm can improve the overall performance of non-differential GNSS/IMU integrated navigation without integer ambiguity resolution. This facilitates high accuracy with a relatively low computational load. The contributions are summarized below:

  1. 1.

    Development of a two-stage filter-based state estimation for GNSS/IMU integrated navigation, with advantages of improving positioning performance without resolving carrier phase inter ambiguity.

  2. 2.

    Design of a cycle slip detection and repair solution and the assistance of full results from TDCP to suppress the position drift and improve accuracy in urban environments. In particular, to reduce the computational load for cycle slip detection and repair, an effective method is designed with the help of Doppler shift measurement. Vehicle motion states estimated from TDCP are all incorporated into the GNSS/INS system, including delta position, velocity, pitch, and heading.

  3. 3.

    Field tests have demonstrated that the proposed algorithm provides a significant improvement over the corresponding traditional algorithm for vehicle operation in urban environments by 39% in position, 31% in velocity and around 40% in attitude.

TDCP-based control input vector construction

Before the depiction of the proposed algorithm, it is necessary to derive the equations for TDCP solution, which is to output the control input vector for input into stage 2 KF later. We first determine the relationship between the receiver delta position and carrier phase measurements for each observed satellite. After this, with the aid of Doppler shift integration, cycle slips can be detected and repaired to achieve a more accurate solution. Finally, the equations of all available satellites at the current epoch are combined to calculate the solution, i.e., delta position, velocity, pitch, and heading.

Relationship between receiver delta position and carrier phase

TDCP measurement, which is the variation of carrier phase over two adjacent epochs, \(t_{k - 1}\) and \(t_{k}\), can be expressed as (Sun et al. 2020):

$$\Delta \Phi = \Phi_{{t_{k} }} - \Phi_{{t_{k - 1} }} = \frac{1}{\lambda }\Delta r + \frac{c}{\lambda }\Delta {\text{d}}t_{R} + \Delta N + \Delta \varepsilon_{\Phi }$$
(1)

Here, \(\Delta \Phi\) is the change in the carrier phase measurements between adjacent epochs in the unit of cycles, while \(\Delta r\) is that in receiver-satellite distance in meters; \(\lambda\) is the wavelength of carrier phase measurements; c is speed of light in a vacuum;\(\Delta {\text{d}}t_{R}\) is the time variation of receiver clock error; \(\Delta N\) is the change in carrier phase ambiguity, which is always zero unless cycle slip occurs; \(\Delta \varepsilon_{\Phi }\) denotes the remaining errors that cannot be eliminated by time differencing, which is usually small enough to neglect. Based on Sun et al. (2020), the time variation in receiver-satellite distance between two epochs can be obtained by:

$$\Delta r = r_{{t_{k} }} - r_{{t_{k - 1} }} = \vec{r}_{{t_{k} }} \cdot \left( {\Delta r^{S} - \Delta r^{R} } \right)$$
(2)

where \(\Delta r^{S}\) and \(\Delta r^{R}\) are the delta positions of the satellite and receiver, respectively, between adjacent epochs in Earth-Centered Earth-Fixed frame (e-frame);\(\vec{r}_{{t_{k} }}\) is the unit vector pointing to the satellite position from the user position and \(\cdot\) is the inner product of vectors. Based on (1) and (2), \(\Delta \Phi\) can be represented as:

$$\Delta \Phi = \frac{1}{\lambda }\vec{r}_{{t_{k + 1} }} \cdot \left( {\Delta r^{S} - \Delta r^{R} } \right) + \frac{c}{\lambda }\Delta {\text{d}}t_{R} + \Delta N + \Delta \varepsilon_{\Phi }$$
(3)

Equation (3) can then be rewritten into a matrix–vector equation as:

$$\left[ {\vec{r}_{{t_{k + 1} }} - 1} \right] \cdot \left[ {\begin{array}{*{20}c} {\Delta r^{R} } \\ {c\Delta {\text{d}}t_{R} } \\ \end{array} } \right] = \vec{r}_{{t_{k + 1} }} \cdot \Delta r^{S} - \lambda \Delta \Phi + \lambda \Delta N + \lambda \Delta \varepsilon_{\Phi }$$
(4)

where \(\Delta r^{R}\) and \(c\Delta {\text{d}}t_{R}\) are unknowns while the remaining variables are knowns or alternatively easily achieved based on knowns except for \(\Delta N\). When the number of common visible satellites for the last and current epochs is not less than four, the equations can berganized as follows:

$$\left[ {\begin{array}{*{20}c} {\begin{array}{*{20}c} {\vec{r}_{{t_{k + 1} }}^{1} } \\ {\vec{r}_{{t_{k + 1} }}^{2} } \\ \end{array} } & {\begin{array}{*{20}c} { - 1} \\ { - 1} \\ \end{array} } \\ {\begin{array}{*{20}c} \vdots \\ {\vec{r}_{{t_{k + 1} }}^{i} } \\ \end{array} } & {\begin{array}{*{20}c} \vdots \\ { - 1} \\ \end{array} } \\ \end{array} } \right] \cdot \left[ {\begin{array}{*{20}c} {\Delta r^{R} } \\ {c\Delta {\text{d}}t_{R} } \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} {\begin{array}{*{20}c} {\vec{r}_{{t_{k + 1} }}^{1} \cdot \Delta r^{S1} - \lambda \Delta \Phi^{1} } \\ {\vec{r}_{{t_{k + 1} }}^{2} \cdot \Delta r^{S2} - \lambda \Delta \Phi^{2} } \\ \end{array} } \\ {\begin{array}{*{20}c} \vdots \\ {\vec{r}_{{t_{k + 1} }}^{i} \cdot \Delta r^{Si} - \lambda \Delta \Phi^{i} } \\ \end{array} } \\ \end{array} } \right] + \lambda \left[ {\begin{array}{*{20}c} {\begin{array}{*{20}c} {\Delta N^{1} } \\ {\Delta N^{2} } \\ \end{array} } \\ {\begin{array}{*{20}c} \vdots \\ {\Delta N^{i} } \\ \end{array} } \\ \end{array} } \right] + \lambda \left[ {\begin{array}{*{20}c} {\begin{array}{*{20}c} {\Delta \varepsilon_{\Phi }^{1} } \\ {\Delta \varepsilon_{\Phi }^{2} } \\ \end{array} } \\ {\begin{array}{*{20}c} \vdots \\ {\Delta \varepsilon_{\Phi }^{i} } \\ \end{array} } \\ \end{array} } \right]$$
(5)

Let \(B = \left[ {\begin{array}{*{20}c} {\begin{array}{*{20}c} {\vec{r}_{{t_{k + 1} }}^{1} } \\ {\vec{r}_{{t_{k + 1} }}^{2} } \\ \end{array} } & {\begin{array}{*{20}c} { - 1} \\ { - 1} \\ \end{array} } \\ {\begin{array}{*{20}c} \vdots \\ {\vec{r}_{{t_{k + 1} }}^{i} } \\ \end{array} } & {\begin{array}{*{20}c} \vdots \\ { - 1} \\ \end{array} } \\ \end{array} } \right]\), \(C = \left[ {\begin{array}{*{20}c} {\begin{array}{*{20}c} {\vec{r}_{{t_{k + 1} }}^{1} \cdot \Delta r^{S1} - \lambda \Delta \Phi^{1} } \\ {\vec{r}_{{t_{k + 1} }}^{2} \cdot \Delta r^{S2} - \lambda \Delta \Phi^{2} } \\ \end{array} } \\ {\begin{array}{*{20}c} \vdots \\ {\vec{r}_{{t_{k + 1} }}^{i} \cdot \Delta r^{Si} - \lambda \Delta \Phi^{i} } \\ \end{array} } \\ \end{array} } \right] + \lambda \left[ {\begin{array}{*{20}c} {\begin{array}{*{20}c} {\Delta N^{1} } \\ {\Delta N^{2} } \\ \end{array} } \\ {\begin{array}{*{20}c} \vdots \\ {\Delta N^{i} } \\ \end{array} } \\ \end{array} } \right]\), using the Least Squares method (LSM), the unknowns can be solved:

$$\left[ {\begin{array}{*{20}c} {\Delta r^{R} } \\ {c\Delta dt_{R} } \\ \end{array} } \right] = \left( {B^{T} B} \right)^{ - 1} B^{T} C$$
(6)

where \(\Delta r^{R} = \left[ {\begin{array}{*{20}c} {\Delta x^{R} } & {\Delta y^{R} } & {\Delta z^{R} } \\ \end{array} } \right]^{T}\) are variations of vehicle positions for adjacent epochs in three-axis directions of the e-frame. A real test example is performed ignoring \(\Delta N\) (i.e. assuming \(\Delta N\) is always zero) and the unknowns \(\Delta r^{R}\) are shown in Fig. 1. The sharp spikes before epoch 100 are likely caused by the impact of \(\Delta N\), which should be a positive or negative integer if cycle slips exist. Therefore, detecting and repairing cycle slips between adjacent epochs efficiently and correctly is crucial for accurate and reliable solutions during TDCP processes. We use the Doppler shift measurements to detect and repair cycle slips. The performance of the cycle slip detection and repairing can be seen in Fig. 1 (right). After correcting the cycle slips, these sharp spikes disappear. The detail of cycle slip detection and repairing is introduced in the next subsection.

Fig. 1
figure 1

Components of delta position in e-frame under both cases. The changes in position without cycle slip detection (left) and with cycle slip detection and repair (right)

Full size image

Doppler integration-aided cycle slip detection and repairing

For each satellite available for TDCP solution, it is uncertain whether the carrier phase integer ambiguity varies (i.e. cycle slip exists). Once a cycle slip occurs for a certain satellite, an equal variation can be contained in the carrier phase integer ambiguity in the current and subsequent epochs. Because the derivation of TDCP relies on the carrier phase measurements at the last and current epochs, the occurrence of cycle slip deteriorates the TDCP solution for the current epoch but has no effect on subsequent epochs.

As the Doppler shift is not affected by cycle slip, the relationship between the Doppler shift and carrier phase is considered. Doppler integration is obtained via the integration of Doppler shift over time, reflecting the total displacement of the user position relative to the satellite position during the interval. The carrier phase measurement is equivalent to adding an unknown integer ambiguity to the Doppler integration. Unless the carrier tracking loop has signal loss of lock or phase loss, TDCP is just the same value as the difference of the Doppler integration between two epochs, free of cycle slip (Silva 2013). Thus, a Doppler integration-aided method is used to detect and repair the cycle slip. The difference in Doppler integration between epochs \(t_{k}\) and \(t_{k + 1}\) can be presented as:

$${\text{d}}\Phi = - \mathop \smallint \limits_{{t_{k} }}^{{t_{k + 1} }} D{\text{d}}t$$
(7)

where \(D\) denotes the Doppler shift. Then the difference between TDCP and \({\text{d}}\Phi\) can be expressed as

$$V = \Delta \Phi - {\text{d}}\Phi = \Delta \Phi + \mathop \smallint \limits_{{t_{k} }}^{{t_{k + 1} }} D{\text{d}}t = \Delta N + \varepsilon$$
(8)

Equation (8) can be simplified with numerical integration to:

$$V = \Delta \Phi + \left( {D_{{t_{k + 1} }} + D_{{t_{k} }} } \right)\left( {t_{k + 1} - t_{k} } \right)/2 = \Delta N + \varepsilon$$
(9)

where \(D_{{t_{k + 1} }}\) and \(D_{{t_{k} }}\) are the Doppler shift measurements at epoch \(t_{k + 1}\) and \(t_{k}\), respectively; \({\text{d}}t = t_{k + 1} - t_{k}\) is the sampling interval. The remaining errors are mostly a few centimeters in magnitude and much less than one cycle slip, summarized by the term \(\varepsilon\). Then, the mathematical expectation and covariance of \(V\) can be calculated as:

$$E\left( V \right) = \Delta N + E\left( \varepsilon \right)$$
(10)
$${\text{Cov}}\left( V \right) = {\text{Cov}}\left( \varepsilon \right)$$
(11)

Instead of the common computation method based on statistical testing that requires redundancy, a real-time recursion-based method is used to calculate the mean \(\overline{V}_{k}\) and variance \(\sigma_{k}^{2}\) of \(V\) for the current epoch (Ren et al. 2012), denoted as:

$$\overline{V}_{k} = \overline{V}_{k - 1} + \frac{1}{k}\left( {V_{k} - \overline{V}_{k - 1} } \right)$$
(12)
$$\sigma_{k}^{2} = \frac{k - 2}{{k - 1}}\sigma_{k - 1}^{2} + \frac{1}{k}\left( {V_{k} - \overline{V}_{k - 1} } \right)^{2}$$
(13)

Then the detection of cycle slip can be executed by:

$$\left| {V_{k} - \overline{V}_{k} } \right| \le m \cdot \sigma_{k}$$
(14)

Here, \(m\) is a crucial parameter to determine the ability to detect cycle slips, and the value of m is set at 3 according to the 3σ principle. If Eq. (14) holds, it is considered that there is no cycle slip; Otherwise, the following equation is used to determine the size of the cycle slip so as to repair the cycle slip:

$$\Delta N = {\text{round}}\left( {V_{k} - \overline{V}_{k} } \right)$$
(15)

Here, \({\text{round}}\left( { } \right)\) rounds the element in the parenthesis towards the nearest integer.

Calculation of TDCP derivations

After solving the cycle slip problem, the vehicle delta position \(\Delta r^{R}\) in the e-frame can be recalculated based on Eq. (6). Thereafter, the vehicle position variation \({\Delta }p\) and velocity \(v_{{{\text{GPS}}_{{{\text{mid}}}} }}\) in the navigation frame (n-frame) can be obtained by (Sun et al. 2020):

$${\Delta }p = R\Delta r^{R}$$
(16)
$$v_{{{\text{GPS}}_{{{\text{mid}}}} }} = \Delta p/\left( {t_{k + 1} - t_{k} } \right)$$
(17)

Here, \(R\) is the coordinate-transformation matrix from e-frame to n-frame; \(v_{{{\text{GPS}}_{{{\text{mid}}}} }} = \left[ {\begin{array}{*{20}c} {v_{{{\text{GPS}}}}^{E} } & {v_{{{\text{GPS}}}}^{N} } & {v_{{{\text{GPS}}}}^{U} } \\ \end{array} } \right]^{T}\), \(v_{{{\text{GPS}}}}^{E}\), \(v_{{{\text{GPS}}}}^{N}\) and \(v_{{{\text{GPS}}}}^{U}\) are the velocity components in the three-axis directions of the n-frame, respectively, which can be viewed as average velocity between adjacent epochs. The \(\psi_{{{\text{GPS}}_{{{\text{mid}}}} }}\), containing pitch (in rad) and heading (in rad) of a vehicle in n-frame, can be calculated based on \(v_{{{\text{GPS}}_{{{\text{mid}}}} }}\) as:

$${\text{pitch}} = \tan^{ - 1} (v_{{{\text{GPS}}}}^{U} /\sqrt {\left( {v_{{{\text{GPS}}}}^{E} } \right)^{2} + \left( {v_{{{\text{GPS}}}}^{N} } \right)^{2} }$$
(18)
$${\text{heading}} = \left\{ {\begin{array}{*{20}c} {\cos^{ - 1} \left( {v_{{{\text{GPS}}}}^{N} /\sqrt {\left( {v_{{{\text{GPS}}}}^{E} } \right)^{2} + \left( {v_{{{\text{GPS}}}}^{N} } \right)^{2} } } \right)} & {v_{{{\text{GPS}}}}^{E} \ge 0} \\ {2{\uppi } - \cos^{ - 1} \left( {v_{{{\text{GPS}}}}^{N} /\sqrt {\left( {v_{GPS}^{E} } \right)^{2} + \left( {v_{{{\text{GPS}}}}^{N} } \right)^{2} } } \right) } & {v_{{{\text{GPS}}}}^{E} < 0} \\ \end{array} } \right.$$
(19)

where \(pitch\in (-\frac{\pi }{2},\frac{\pi }{2})\), and \(heading\in [\mathrm{0,2\pi })\).

Algorithm design

The flowchart for the proposed GNSS/IMU integrated algorithm is presented in Fig. 2. The algorithm includes two parts: TDCP derivation and Kalman filtering-based GNSS/IMU integration. First, the GNSS receiver outputs the observation and navigation files. The three main observations of pseudorange, carrier phase, and Doppler shift from the observation file, as well as the ephemeris parameters related to the satellites and atmospheric propagation from the navigation file, are used.

Fig. 2
figure 2

Proposed TDCP/IMU integrated scheme. The TDCP derivation process for calculating of the delta position, velocity and attitude of vehicle (right) and the calculated parameters for Kalman filter-based GNSS/IMU integration (left)

Full size image

Specific force and angular rate from the IMU output are processed by INS mechanization and further integrated with results of pseudorange-based positioning by LSM as the first stage of the filter to obtain the initial state estimation. Meanwhile, TDCP from the carrier phase is checked for cycle slip with the aid of Doppler shift, as well as change in satellite positions and unit vectors pointing from receiver to the satellites, to calculate the control vector (i.e., relative position, velocity, heading and pitch of the vehicle), depicted in the last section. Eventually, the final state estimation from stage 2 KF can be obtained by analyzing the relationship between the initial state estimation and the control input vector. The process of the two-stage filter is detailed in the following subsection.

Two-stage filter-based state estimation for integration

For a better description of the update interval in the two-stage filter, the sampling rates of GNSS and IMU are denoted as m Hz and n Hz, respectively. Since the sampling rate of IMU is generally an integer multiple of that of GNSS, it is sensible that the EKF-based GNSS/IMU integration does not predict and update simultaneously for every epoch. Hence, there are n/m IMU sampling intervals between adjacent consecutive GNSS epochs. Also, the execution interval for TDCP is the same as the GNSS sampling rate. The relation between GNSS and IMU in the time interval is depicted in Fig. 3. t represents GNSS epoch while k represents IMU time. Assuming that t for GNSS and k for IMU are at the same time, k − n/m and t − 1 are the same as well, the same for k + n/m and t + 1.

Fig. 3
figure 3

Time intervals for GNSS and IMU. The IMU sampling rate is n/m times than that of GNSS. k−n/m, k, and k + n/m represent IMU time, while t−1, t and t + 1 represent GNSS epochs. The upper and lower at the same point on the axis correspond to the same time

Full size image

Stage 1: EKF-based GNSS/IMU initial state estimation.

With the time intervals defined above, an Extended Kalman Filter (EKF) based on the linearization of nonlinear models is specified in the initial fusion stage. The state transition and measurement equations are constructed as follows:

$${\text{X}}_{k} = \emptyset_{k,k - 1} {\text{X}}_{k - 1} + w_{k - 1}$$
(20)
$$Z_{k} = HX_{k} + v_{k}$$
(21)

where \({\mathrm{\varnothing }}_{k,k-1}\) and \(H\) are the state transition and measurement matrices, respectively; \({X}_{k}\) and \({X}_{k-1}\) are the state vector \(X\) in the time epochs k and k − 1, respectively; \(w_{k - 1}\) is the process noise \(w\) in the time epoch k−1 with the covariance matrix \(Q\) while \(v_{k}\) is the measurement noise \(v\) in the time epoch k with the covariance matrix \(R\); \(Z_{k}\) is the measurement \(Z\) at epoch k. Covariance Q is given based on the standard deviations of the accelerometers and gyroscope. R is related to the variances of GNSS’s position. Based on parameters of IMU type, the spectral density matrix \(Q_{0}\) can be denoted using white noise as:

$$Q_{0} = {\text{diag}}\left( {\begin{array}{*{20}c} {\begin{array}{*{20}c} 0 & {\sigma_{vrw}^{2} I} & {\begin{array}{*{20}c} {\sigma_{arw}^{2} I} & {\frac{{2\sigma_{bg}^{2} }}{{T_{bg} }}I} \\ \end{array} } \\ \end{array} } & {\begin{array}{*{20}c} {\frac{{2\sigma_{ba}^{2} }}{{T_{ba} }}I} & {\frac{{2\sigma_{sg}^{2} }}{{T_{sg} }}I} & {\frac{{2\sigma_{sa}^{2} }}{{T_{sa} }}I} \\ \end{array} } \\ \end{array} } \right)$$
(22)

Herein, zero matric and all identity matrices are of size of \(3 \times 3\); \(\sigma_{vrw}^{2}\) and \(\sigma_{arw}^{2}\) are variance of random walk noise for accelerometer and gyro, respectively; \(T_{ba}\) and \(\sigma_{ba}^{2}\) are the correlation time and variance of accelerometer bias, while \(T_{bg}\) and \(\sigma_{bg}^{2}\) are those of gyro bias; \(T_{sa}\) and \(\sigma_{sa}^{2}\) are the correlation time and variance of accelerometer scale factor, while \(T_{sg}\) and \(\sigma_{sg}^{2}\) are those of the gyro scale factor.

Then, the system noise covariance matrix \(Q\) can be expressed:

$$Q = \emptyset GQ_{0} G^{T} \emptyset^{T} \Delta t$$
(23)

where \(\emptyset\) is the transition matrix; \(G\) is the design matrix of system noise vector; \(\Delta t\) is the time interval.

Measurement noise matrix can be denoted as:

$$R = {\text{diag}}\left( {\begin{array}{*{20}c} {\sigma_{x}^{2} } & {\sigma_{y}^{2} } & {\sigma_{z}^{2} } \\ \end{array} } \right)$$
(24)

where \(\sigma_{x}^{2}\), \(\sigma_{y}^{2}\) and \(\sigma_{z}^{2}\) are variances of GNSS three-axis position obtained from GNSS processing.

The position and velocity errors, the bias and scale factor of both gyroscope and accelerometer are all augmented into the state vector, modeled as first-order Gauss–Markov process (Park 2004), defined as:

$$X = \left[ {\begin{array}{*{20}c} {\begin{array}{*{20}c} {\delta r^{n} } & {\delta v^{n} } & \varepsilon \\ \end{array} } & {\begin{array}{*{20}c} {b_{g} } & {b_{a} } \\ \end{array} } & {\begin{array}{*{20}c} {s_{g} } & {s_{a} } \\ \end{array} } \\ \end{array} } \right]^{T}$$
(25)

where \(\delta r^{n}\) and \(\delta v^{n}\) denote vectors of position and velocity errors in the n-frame, respectively; \(\varepsilon\) is the vector attitude (i.e. roll, pitch and yaw) error; \(b_{g}\) and \(s_{g}\) are the gyro vectors of bias and gyro scale factor errors while \(b_{a}\) and \(s_{a}\) are the corresponding accelerometer errors. The state transition matrix is set based on the error propagation of IMU, derived from the IMU mechanization.

The measurement vector can be defined as:

$$Z = \left[ {\begin{array}{*{20}c} {x^{{{\text{IMU}}}} - x^{{{\text{GNSS}}}} } \\ {y^{{{\text{IMU}}}} - y^{{{\text{GNSS}}}} } \\ {z^{{{\text{IMU}}}} - z^{{{\text{GNSS}}}} } \\ \end{array} } \right]$$
(26)

where \(\left( {x,y,z} \right)\) is the receiver position in the n-frame and superscripts ‘IMU’ and ‘GNSS’ represent the information from IMU and GNSS, respectively. The receiver positions from the IMU are calculated from IMU mechanization while those from GNSS are obtained by single point positioning. Then the measurement matrix is composed of units 1 and 0, where 1 corresponds to the mapping of the position error from the first three units in the state vector to the measurement vector.

Here, the EKF contains two phases: prediction and update. The procedures for the two can be presented as follows:

prediction phase:

$${\text{X}}_{k} = \emptyset_{k,k - 1} {\text{X}}_{k - 1}$$
(27)
$$P_{k,k - 1} = \emptyset_{k,k - 1} P_{k - 1} \emptyset_{k,k - 1}^{T} + Q_{k - 1}$$
(28)

update phase:

$$K_{k} = P_{k,k - 1} H_{k}^{T} \left( {H_{k} P_{k,k - 1} H_{k}^{T} + R_{k} } \right)^{ - 1}$$
(29)
$$P_{k} = \left( {I - K_{k} H_{k} } \right)P_{k,k - 1}$$
(30)
$$\hat{X}_{k} = \hat{X}_{k,k - 1} + K_{k} \left( {Z_{k} - H_{k} \hat{X}_{k,k - 1} } \right)$$
(31)

In the prediction phase, the IMU errors are continuously predicted according to the error propagation law of IMU sensors, later fed back to correct the parameters in IMU mechanization. However, the update phase only executes epochs with GNSS observations and thus helping to suppress the divergence of inertial navigation errors. The details of EKF algorithm can be found in Hwang et al. (2005).

Stage 2: KF-based state estimation with TDCP solution aiding.

Stage 2 is executed immediately after stage 1 in the two-stage filter. The prediction and update phase are synchronized in this stage, depending on the output rate of TDCP. To distinguish from those in stage one, the state vector \(Y\) and measurement vector \({\text{W}}\) are, respectively, expressed as:

$$Y = \left[ {\begin{array}{*{20}c} p & v & \phi \\ \end{array} } \right]^{T}$$
(32)
$${\text{W}} = \left[ {\begin{array}{*{20}c} {p_{{{\text{com}}}} } & {v_{{{\text{com}}}} } & {\phi_{{{\text{com}}}} } \\ \end{array} } \right]^{T}$$
(33)

where \(p,v,\phi\) represent position, velocity and attitude (only including pitch and heading here), respectively, and the subscript ‘com’ represents those from the initial GNSS/IMU combination.

The principle for the state transition construction in stage 2 can be described as follows. The control input vector has been obtained before algorithm design. Within the vector, \({\Delta }p\) is the variation of position from the previous epoch to the current epoch. With the change in position is used to determine the position at the current epoch. \(v_{{GPS_{mid} }}\) is the average velocity for the two adjacent epochs. Over a very short period, a uniform linear vehicle acceleration can be assumed. Hence, \(v_{{{\text{GPS}}_{{{\text{mid}}}} }}\) can also be assumed as the instantaneous velocity at the intermediate moment, which is the sum of the initial and final velocities divided by two (just like a normal average) according to the nature of uniform acceleration linear motion. With the velocity of the previous epoch and average velocity known, the velocity for the current epoch can be obtained. Similar to velocity, the attitude \(\psi_{{{\text{GPS}}_{{{\text{mid}}}} }}\) at the current epoch can be obtained as well. Then the state transition and measurement matrices are presented as:

$$Y_{t} = F_{t,t - 1} Y_{t - 1} + B_{t - 1} u_{t - 1} + w_{t - 1}^{\prime }$$
(34)
$$W_{t} = G_{t} Y_{t} + v_{t}^{\prime }$$
(35)

where \(F_{t,t - 1} = \left[ {\begin{array}{*{20}c} {I_{3} } & {0_{3 \times 5} } \\ {0_{5 \times 3} } & { - I_{5} } \\ \end{array} } \right]\),\({\text{ G}}_{t} = I_{8}\), \(I_{n}\) is the n-by-n identity matrix and \(0_{m \times n}\) is an m-by-n matrix of zeros; \(w_{t - 1}^{\prime }\) and \(v_{t}^{\prime }\) are process noise and measurement noise with covariance \(Q^{\prime }\) and \(R^{\prime }\), respectively. The deterministic driving term \(B_{t - 1} u_{t - 1}\) is composed of a control input vector \(B_{t - 1}\) and control input matrix \(u_{t - 1}\).

$$B_{t - 1} = \left[ {\begin{array}{*{20}c} {I_{3} } & {0_{3 \times 5} } \\ {0_{5 \times 3} } & {2 \times I_{5} } \\ \end{array} } \right]$$
(36)
$$u_{t - 1} = \left[ {\begin{array}{*{20}c} {{\Delta }p\left( {t - 1} \right)} & {v_{{{\text{GPS}}_{{{\text{mid}}}} }} \left( {t - 1} \right)} & {\begin{array}{*{20}c} {\theta_{{{\text{GPS}}_{{{\text{mid}}}} }} \left( {t - 1} \right)} & {\psi_{{{\text{GPS}}_{{{\text{mid}}}} }} \left( {t - 1} \right)} \\ \end{array} } \\ \end{array} } \right]^{T}$$
(37)

Here, \(u_{t - 1}\) is the TDCP derivations obtained based on measurements at epoch \(t - 1\) and \(t\). Considering the relationship between stage 1 and stage 2, some matrices in stage 2 can be inherited from stage 1, including the state matrix \(Y_{t - 1}\) and measurement matrix \(W_{t}\), denoted as follows:

$$Y_{t - 1} = \left[ {\begin{array}{*{20}c} {p_{{{\text{com}}}} \left( {t - 1} \right)} & {v_{{{\text{com}}}} \left( {t - 1} \right)} & {\phi_{{{\text{com}}}} \left( {t - 1} \right)} \\ \end{array} } \right]$$
(38)
$$W_{t} = \left[ {\begin{array}{*{20}c} {p_{{{\text{com}}}} \left( t \right)} & {v_{{{\text{com}}}} \left( t \right)} & {\phi_{{{\text{com}}}} \left( t \right)} \\ \end{array} } \right]$$
(39)

For the current stage of the filter, the optimal estimate at epoch \(t - 1\) used to predict the state for epoch \(t\) through the first-order Markov process and the observation in epoch \(t\) are, respectively, derived from the optimal estimate for two adjacent epochs in stage 1. Therefore, the process noise covariance matrix \(Q_{k - 1}^{\prime }\) is composed of the corresponding elements in \(P_{k - n/m}\) while the measurement noise covariance matrix \(R_{t}^{\prime }\) is composed of the corresponding elements in \(P_{k}\) from stage 1. Then the prediction and update processes can be executed according to the following equations:

$$P_{t,t - 1}^{\prime } = {\text{F}}_{t,t - 1} P_{t - 1}^{\prime } {\text{F}}_{t,t - 1}^{T} + Q_{k - 1}^{\prime }$$
(40)
$$K_{t}^{\prime } = P_{t,t - 1}^{\prime } G_{t}^{T} \left( {G_{t} P_{t,t - 1}^{\prime } G_{t}^{T} + R_{t}^{\prime } } \right)^{ - 1}$$
(41)
$$P_{t}^{\prime } = \left( {I - K_{t}^{\prime } G_{t} } \right)P_{t,t - 1}^{\prime }$$
(42)

where \(P_{t,t - 1}^{\prime }\) and \(P_{t}^{\prime }\) are the covariance matrices of predicted state and final estimated state, respectively; \(K_{t}^{\prime }\) is the Kalman gain. Then the final state estimation \(\hat{Y}_{t}\) can be obtained by:

$$\hat{Y}_{t} = F_{t,t - 1} Y_{t - 1} + B_{t - 1} u_{t - 1} + K_{t}^{\prime } \left( {W_{t} - G_{t} F_{t,t - 1} Y_{t - 1} - G_{t} B_{t - 1} u_{t - 1} } \right)$$
(43)

\(\hat{Y}_{t}\) will be output to user providing the position, velocity and attitude of the vehicle.

Experimental process

In order to evaluate the performance of the proposed algorithm, the results are compared with traditional GNSS/IMU fusion. A vehicle field test using a Novatel PwrPak7 GNSS receiver and a Bosch BMI055 IMU was performed on the ground in an urban environment in Kaohsiung City, Taiwan, China (Fig. 4), to validate the proposed algorithm. The GNSS raw single frequency measurements were collected at a sampling rate of 1 Hz, including pseudorange, carrier phase as well as Doppler measurements from two satellite constellations (i.e., GPS and BDS), while the IMU raw data were collected at a sampling rate of 50 Hz, with specific force and angular rate measurements used. The reference of the trajectory used in the experiment was determined by post-processing of data from an integrated high grade GNSS receiver/IMU (iNAV-RQH from i-Mar) with the commercial software NovAtel Inertial Explorer. The equipment layout is shown in Fig. 5. Table 1 lists the specifications of the IMUs used.

Fig. 4
figure 4

Novatel PwrPak7 GNSS receiver (left) and Bosch BMI055 IMU (right)

Full size image
Fig. 5
figure 5

Experimental layout for the iNAV-RQH reference system

Full size image
Table 1 Performance characteristics of IMUs
Full size table

The test environment was on the highway in a light-urban environment, with the experimental route shown in Fig. 6. During the test, although the passing by vehicles, trees and buildings on the way will obstruct some of the visible satellites at some epochs, the most of epochs are with more than 9 visible satellites for each constellation and also with low position dilution of precision (PDOP) values. Figure 7 illustrates the quality of the GNSS solution, including the number of satellites in view for each epoch (left) and the PDOP value (right), which reflects the geometric distribution of observed satellites with a high value reflecting weak geometry. The total test duration was approximately 480 s and GPS + BDS measurements were captured.

Fig. 6
figure 6

Experimental route

Full size image
Fig. 7
figure 7

Number of GPS and BDS satellites in view (left) and PDOP at each epoch (right) in the experimental test

Full size image

Results analysis and discussion

The performance provided by the proposed GNSS/IMU fusion scheme is compared with that of the traditional GNSS/IMU fusion scheme. The traditional GNSS/IMU algorithm is without the aiding of TDCP, which only contains the first stage filtering process of the proposed algorithm. The results show that the proposed algorithm is superior to the traditional one, due to added value of TDCP. The positioning performance of the proposed algorithm exerts great improvements over the traditional method and GNSS pseudorange positioning method, shown in Fig. 8. Around epoch 40, the positioning accuracy of the proposed algorithm is significantly increased in the horizontal direction. At the same time, effectiveness is reflected in constraining the divergence of elevation, especially around epochs 350 and 400, when the traditional combined system may be interfered with, resulting in the instability in the height component. Figure 9 shows the comparison of velocity errors, whose distribution is similar to that of position errors. Because the position is derived from velocity in IMU mechanization while in TDCP the velocity is directly calculated from the change in position, the velocity change drives the position change and vice versa. The attitude errors are depicted in Fig. 10. Relatively accurate pitch and heading derived from TDCP are input into the integrated system, bringing an additional correction for attitude. Meanwhile, attitude estimation can be affected by the velocity correction from TDCP as well. Additionally, during the beginning period in Figs. 8, 9, and 10, it also can be found that the proposed integrated navigation system tends converge more quickly than traditional one, with the aid of accurate velocity and attitude derived from TDCP.

Fig. 8
figure 8

Comparison of position error in test case

Full size image
Fig. 9
figure 9

Comparison of velocity error in test case

Full size image
Fig. 10
figure 10

Comparison of attitude error in test case

Full size image

The performance comparison of the proposed algorithm and traditional GNSS/IMU combined algorithm for the test case in terms of RMSE are shown in Table 2. The RMSE in the horizontal and vertical positions are 2.58 m and 1.38 m, respectively, an increase of 23.5% and 59.1% when compared to those of the traditional method of 3.38 m and 3.37 m. Velocity RMSEs are increased to 0.21 m/s, 0.41 m/s and 0.19 m/s in east, north and up directions, respectively, corresponding to improvements of 23.6%, 29.6% and 39.8%. The accuracy of attitude components of pitch and roll are 0.79 degrees and 0.57 degrees, both within 1 degree, with improvements of 35.5% and 44.8%, respectively. The improvement in the heading is 39.5%, RMSE from 4.10 degrees to 2.48 degrees. Although the roll angle is not fused in the second-stage filter, it could be affected by the other errors during the first-stage filter due to the correlation between units in the state vector. The running times for the test case with the two algorithms are compared in Table 3. Although two times filter is adopted in the proposed algorithm, the total time is 13.08 s, only 1.47 s longer than the traditional GNSS/IMU fusion scheme. This is because the two stages of the proposed algorithm are both based on the Kalman filter, whose efficiency is quite high. Although the stage 1 filter adopted the Extended Kalman filter, its linearization process can be pre-calculated. Therefore, the change in computational efficiency of the algorithm can be ignored when compared to that of the traditional GNSS/IMU algorithm.

Table 2 Performance comparison of RMSE for the test case
Full size table
Table 3 Computational efficiency of the algorithms for the test case
Full size table

Summary and conclusions

We have developed a TDCP-aided GNSS/IMU fusion scheme incorporating carrier phase cycle slip detection and repairing. Field test results on a highway in a mid-urban urban environment show that the proposed fusion scheme can improve vehicle state estimation over the traditional GNSS/IMU algorithm in the accuracy in position, velocity and attitude of about 39%, 31 and 40%, respectively. It should be noted, however, that although the proposed TDCP-based algorithm can improve the performance of the integrated navigation system, the accuracy of the algorithm is restricted to the sampling rate of GNSS data. Thus, improvement of the proposed algorithm can be expected with a higher sampling rate as well as a flexible process interval of TDCP. In the future, the experiment in deep urban areas will be carried out to validate the effectiveness of the proposed algorithm. In that case, with larger errors in pseudorange and carrier phase, quality control methods for GNSS measurements may be developed to improve the algorithm performance. Fault detection and isolation, as well as the adaptive weighting strategies can also be used in the quality control of GNSS measurements. In addition, the performance may be improved by designing a tightly coupled-based fusion scheme, in which the TDCP derivation may be used for the pseudorange and pseudorange rate prediction.