High-Accuracy Positioning in GNSS-Blocked Areas by Using the MSCKF-Based SF-RTK/IMU/Camera Tight Integration

Abstract

The integration of global navigation satellite system (GNSS) single-frequency (SF) real-time kinematics (RTKs) and inertial navigation system (INS) has the advantages of low-cost and low-power consumption compared to the multiple-frequency GNSS RTK/INS integration system. However, due to the vulnerability of GNSS signal reception, the application of the GNSS SF-RTK/INS integration is limited in complex environments. To improve the positioning accuracy of SF-RTK/INS integration in GNSS-blocked environments, we present a low-cost tight integration system based on BDS/GPS SF-RTK, a low-cost inertial measurement unit (IMU), and a monocular camera. In such a system, a multi-state constraint Kalman filter (MSCKF) is adopted to integrate the single-frequency pseudo-range, phase-carrier, inertial measurements, and visual data tightly. A wheel robot dataset collected under satellite signal-blocked conditions is used to evaluate its performance in terms of position, attitude, and run time, respectively. Results illustrated that the presented model can provide higher position accuracy compared to those provided by the RTK/INS tight integration system and visual-inertial tight integration system. Moreover, the average running time presents the potential of the presented method in real-time applications.

1. Introduction

The precision position is essential to fields like autonomous driving and mobile mapping [1]. Among existing positioning methods, single-frequency (SF) real-time kinematic (RTK) based on global navigation satellite system (GNSS) observations is a positioning technique with relatively low cost. Theoretically, users can obtain positioning results with centimeter-level accuracy by using the GNSS SF-RTK method [2,3,4]. However, such a high-accuracy character would be significantly degraded or even lost under weak GNSS signal areas (i.e., urban, canyon, and forest) [5,6,7].

Fortunately, the inertial navigation system (INS), an active navigation technology, can sustain short-term high accuracy under GNSS-denied environments. To overcome the drawbacks of satellite positioning technology, the integration scheme of GNSS and INS has been explored by many researchers. In [8], an extended Kalman filter (EKF) was used to integrate GNSS and INS loosely. Results showed that such a system can provide continuous and reliable centimeter-level positioning results in open-sky environments. In [9], the maximum likelihood principle, unscented Kalman filter (UKF), and the process noise covariance estimation model were adopted in a GNSS/INS loose integration system. Results showed that the proposed model can effectively limit the impact of uncertainty of process noise and further improve positioning accuracy. However, the precondition of the GNSS/INS loose integration system is to obtain the GNSS solutions. It needs enough observed satellites. To fully utilize the GNSS observations, some researchers have built a tight integration system based on GNSS observations and IMU measurements. The work in [10] showed that the positioning performance of GPS RTK/INS tight integration is higher than that of loose integration after simulating GPS satellite outages. In [11], a single-frequency multi-GNSS RTK/INS tight integration model was presented. Results showed that the model can provide centimeter-level positioning results in complex urban environments. An RTK/INS tight integration system with adaptability and robustness was designed in [12], and experiment results showed the impacts of state system error and observational error can be constrained.

These studies present the feasibility of using GNSS/INS integration to upgrade positioning accuracy and continuity. However, the GNSS/INS integration will degenerate into an INS-only while the GNSS observations are lost. Under such a condition, the positioning performance will rapidly decrease due to IMU hardware errors. With the development of low-cost cameras, the integration of a camera and IMU is considered to be able to obtain high-precision positions. In [13], a visual-inertial tight integration system using nonlinear optimization based on a keyframes strategy and a sliding window was provided. An indoor experiment showed that the loop-closure error is 0.6 m while using the proposed method. Ref. [14] presented a reliable monocular visual-inertial state estimator based on nonlinear optimization by tightly integrating the IMU pre-integration values and feature observations. The functions of online relocation and robust initialization are also integrated. Results showed that the solutions of position and attitude are more accurate and stable than those from OKVINS [13] while using the presented method. Different from the methods using the pre-integrated IMU observations, the work in [15] integrated loop-closing constraints and nonlinear factors extracted by VIO to perform global nonlinear optimization. Results based on the public datasets demonstrated that the proposed method made the roll and pitch angles observable and the pose accuracy was higher than those in VINS mono [14], ORB-SLAM [16], and other methods with lower optimization scale. Although the nonlinear optimization-based approaches present the advantage of handling the system’s nonlinearity, multiple iterations of the optimization system would cost more for computation. Therefore, some researchers have focused on methods based on EKF. In [17], a visual-inertial odometry system based on multi-state constraint Kalman filter (MSCKF) was proposed. This innovative system introduces the camera pose into the state vector instead of the feature points position, which solves the dimensional explosion problem. Results showed that the system can obtain a high-accuracy pose with low computational complexity in a large-scale environment. However, since the Jacobi matrix is calculated using state values at different epochs, the heading angle is erroneously observable which can degrade the accuracy of pose estimation [18]. To solve this problem, Ref. [19] modified the MSCKF algorithm from the perspective of system observability. Results showed that the improved algorithm significantly outperformed the original algorithm and the fixed-lag smoothing (FLS) algorithm [20] in terms of accuracy and consistency. In [21], the inconsistency problem was solved by using a robot-centric approach, and the direct intensity error is used to form the innovation in the filter update phase. Results illustrated that the presented system exhibits accurate tracking and strong robustness in the conditions of disturbances and fast motions. The results in [22] showed that the filter-based approach can obtain a similar estimation accuracy as the nonlinear optimization-based approach.

Although the visual-inertial odometer system can provide relative pose information with high accuracy, the global pose cannot be obtained. Moreover, the positioning performance would be impacted by light intensity and the quality of feature points. To solve this problem, a fusion of GNSS and VIO is suggested. Ref. [23] presented a loose integration of GNSS and VIO based on EKF and meter-lever global position, resulting in complex urban environments can be obtained. In [24], the factors of IMU, visual, and GNSS RTK are tightly integrated by using factor graph optimization. Results illustrate that it can provide continuous, real-time, and robust positioning results in various-degenerated and challenging scenes for a wheeled robot. Ref. [25] showed that an MSCKF-based GNSS/INS/Vision loose model can provide higher accuracy local pose compared to that of GNSS/INS loose integration. To fully utilize the observations, GNSS raw measurements are considered to introduce into the VIO. Ref. [26] integrated GNSS pseudo-range observations, IMU measurements, and visual information in a nonlinear-optimization-based tightly. Decimeter-level position accuracy in open environments and meter-level position accuracy in complex environments can be respectively obtained by using the proposed model. The GNSS pseudo-range rate was also introduced into the VIO system based on an extended Kalman filter in [27]. The proposed model can effectively improve the drift error of VIO and can improve the usability and accuracy of GNSS in urban areas. A PPP-RTK/MEMS/Vision integration model is presented in [28], in which the GNSS phase measurements, MEMS IMU measurements, and visual data are integrated based on MSCKF. The performance of the method is comparable with that of the PPP-RTK/Tactical IMU tight integration system.

In this paper, an MSCKF-based GNSS SF-RTK/INS/Vision (RIV) tight integration model is presented. In such a system, SF GNSS observations including pseudo-range and carrier-phase, IMU measurements, and visual information are tightly integrated. Compared to previous studies, the main contributions of this paper are (1) the GNSS observations including pseudo-range and phase-carrier are directly introduced to construct a robust GNSS RTK/INS/Vision system, which can provide higher-accuracy positioning results in complex scenes; (2) The advantages of applying single-frequency GNSS data in complex environments are elaborated by comparing the performance differences of incorporating single-frequency data and dual-frequency data in the RIV system, respectively. The subsequent structures of this paper are organized as follows: The state model and the measurement model of the presented system are presented in detail in the Methods section. Then, a set of wheel robot data are processed to evaluate the performance of the presented model in Section 3 and Section 4. Finally, the conclusion follows.

2. Methods

In this subsection, the state model and measurement model of GNSS RTK/INS/vision tight integration and the algorithm summary are described in detail.

2.1. State Model

As described in [25], the state model of MSCKF can be illustrated as

$$x_k={\Phi }_{k,k-1}{x}_{k-1}+{\mu }_{k-1}$$

(1)

where ${\Phi }_{k,k-1}$ denotes the transition matrix of error state from epoch k − 1 to k; ${\mu }_{k-1}$ is the parameter state noise vector; $x_k$ stands for the state vector. Compared to the traditional EKF method, MSCKF introduces the camera pose into the state vector instead of the feature points position, which solves the dimensional explosion problem. Therefore, the state vector can be expressed as

$$x_k={[\begin{array}{llllll}{x}_{INS}\hfill & {\phi }_1\hfill & \delta p_1^e\hfill & \cdots \hfill & {\phi }_m\hfill & \delta p_m^e\hfill \end{array}]}^T$$

(2)

$${\mathit{x}}_{INS}={\left[\begin{array}{ccccc}\delta {\mathit{p}}^e& \delta {\mathit{v}}^e& \mathit{\phi }& \delta {\mathit{b}}_a& \delta {\mathit{b}}_g\end{array}\right]}^T$$

(3)

where $\delta {\mathit{p}}^e$, $\delta {\mathit{v}}^e$, and $\mathit{\phi }$ stand for the position, velocity, and attitude error in the earth-centered earth-fixed (E) frame; ${\phi }_j$ and $\delta p_j^e$ ($1\le j\le m$) denote the IMU pose at image j; $\delta {\mathit{b}}_a$ and $\delta {\mathit{b}}_g$ are the bias errors of the accelerometer and gyroscope. Since the IMU pose related to each image does not change with time, the corresponding error states have

$$\{\begin{array}{c}{\dot{\mathit{\phi }}}_j=0\\ \delta {\dot{\mathit{p}}}_j=0\end{array}$$

(4)

According to [29], the error state function of INS in the e-frame can be expressed by

$$\{\begin{array}{c}\delta {\dot{\mathit{p}}}^e=\delta {\mathit{v}}^e\\ \delta {\dot{\mathit{v}}}^e=\left({\mathit{C}}_b^e{\mathit{f}}^b\right)\times \mathit{\phi }+{\mathit{C}}_b^e\delta {\mathit{f}}^b-2{\mathit{\omega }}_{ie}^e\times \delta {\mathit{v}}^e+\delta {\mathit{g}}^e\\ \dot{\mathit{\phi }}=-\left({\mathit{\omega }}_{ie}^e\times \right)\mathit{\phi }-{\mathit{C}}_b^e\delta {\mathit{\omega }}_{\mathrm{ib}}^b\end{array}$$

(5)

where ${\mathit{C}}_b^e$ denotes the rotation matrix from body-frame (b) to e-frame; ${\mathit{f}}^b$ and ${\mathit{\omega }}_{ib}^b$ are accelerometer outputs and gyroscope outputs. ${\mathit{\omega }}_{ie}^e$ is the angle rate of the e-frame with respect to the inertial-frame (i); ${\mathit{g}}^e$ denotes the gravity vector in the e-frame.

To limit the INS drifts over time, IMU sensor errors are estimated online. Generally, the IMU sensor errors are modeled as a first-order Gauss–Markov process.

$$\left(\begin{array}{c}\delta {\dot{b}}_a\\ \delta {\dot{b}}_{\mathit{g}}\end{array}\right)=\left(\begin{array}{c}-\frac{1}{{\tau }_{b_a}}\delta b_a\\ -\frac{1}{{\tau }_{b_{\mathit{g}}}}\delta b_{\mathit{g}}\end{array}\right)+w$$

(6)

where $\tau$ is the correlation time of the process; $w$ is the driving white noise; other symbols are the same as those mentioned above.

Based on Equations (4)–(6), the following error-state functions can be obtained:

$$\left[\begin{array}{c}{\dot{\mathit{x}}}_{INS}\\ ⋮\\ {\dot{\mathit{\phi }}}_{\mathrm{j}}\\ \delta {\dot{\mathit{p}}}_{\mathrm{j}}^e\end{array}\right]=\left[\begin{array}{cccc}\mathit{F}& & & \\ & \ddots & & \\ & & 0& \\ & & & 0\end{array}\right]\left[\begin{array}{c}{\mathit{x}}_{INS}\\ ⋮\\ {\mathit{\phi }}_{\mathrm{j}}\\ \delta {\mathit{p}}_{\mathrm{j}}^e\end{array}\right]+\left[\begin{array}{c}\mu \\ ⋮\\ 0\\ 0\end{array}\right]$$

(7)

where F is the system dynamic matrix, which is derived from Equations (5) and (6). Considering the high frequency of IMU data collection, the following numerical approximation can be applied to calculate the transition matrix [30]:

$${\Phi }_{k,k-1}\approx I+F\Delta t_{k,k-1}$$

(8)

where I denotes the identity matrix; $\Delta t_{k,k-1}$ is the time difference from epoch k − 1 to k.

Once a new image arrives, the current IMU pose can be augmented into the state vector. The corresponding covariance matrix should be also augmented.

$${{\mathit{P}}^{\prime }}_{k,k-1}=\left[\begin{array}{c}{\mathit{I}}_{15+6m}\\ \mathit{J}\end{array}\right]{\mathit{P}}_{k,k-1}{\left[\begin{array}{c}{\mathit{I}}_{15+6m}\\ \mathit{J}\end{array}\right]}^T=\left[\begin{array}{cc}{\mathit{P}}_{k,k-1}& {\mathit{P}}_{k,k-1}{\mathit{J}}^T\\ \mathit{J}{\mathit{P}}_{k,k-1}& \mathit{J}{\mathit{P}}_{k,k-1}{\mathit{J}}^T\end{array}\right]$$

(9)

with

$$\mathit{J}=\left[\begin{array}{llllll}{\mathit{I}}_{3\times 3}\hfill & 0_{3\times 3}\hfill & 0_{3\times 3}\hfill & 0_{3\times 3}\hfill & 0_{3\times 3}\hfill & 0_{3\times 6m}\hfill \\ 0_{3\times 3}\hfill & 0_{3\times 3}\hfill & {\mathit{I}}_{3\times 3}\hfill & 0_{3\times 3}\hfill & 0_{3\times 3}\hfill & 0_{3\times 6m}\hfill \end{array}\right]$$

(10)

where ${\mathit{P}}_k$ and ${{\mathit{P}}^{\prime }}_k$ denote the error state covariance matrices before and after augmentation, respectively; J stands for the Jacobi matrix.

2.2. GNSS Measurement Model

According to [31], the measurement function of EKF can be written as

$${\mathit{z}}_{k,GNS\mathit{S}}={\mathit{H}}_{k,GNSS}{\mathit{x}}_k+{\mathit{v}}_{k,GNSS}$$

(11)

where ${\mathit{z}}_{k,GNSS}$ denotes the innovation vector; ${\mathit{H}}_{k,GNSS}$ is the design coefficient matrix; ${\mathit{v}}_{k,GNSS}$ represents the observation noise vector; ${\mathit{x}}_k$ is the same vector in Equation (1).

For RTK/INS tight integration model, the term of ${\mathit{z}}_k$ has the following formula

$${\mathit{z}}_{k,GNSS}=\left[\begin{array}{c}{\mathit{p}}_{INS}-\nabla \Delta {\mathit{P}}_{GNSS}\\ {\mathit{p}}_{INS}-\lambda \left(\nabla \Delta {\varphi }_{GNSS}+\nabla \Delta {\mathit{N}}_{GNSS}\right)\end{array}\right]+\left[\begin{array}{c}\Vert {\mathit{C}}_n^e\left[\left({\mathit{C}}_b^n{l}_{GNSS}^b\right)\times \right]\mathit{\phi }\Vert \\ \Vert {\mathit{C}}_n^e\left[\left({\mathit{C}}_b^n{l}_{GNSS}^b\right)\times \right]\mathit{\phi }\Vert \end{array}\right]$$

(12)

where ${\mathit{p}}_{INS}$ stands for the INS-predicted visual observations; $\nabla \Delta (\cdot )$ is the double-differenced (DD) operator; ${\mathit{P}}_{GNSS}$ and ${\varphi }_{GNSS}$ denotes the GNSS pseudo-range and carrier-phase observations; $\lambda$ and ${\mathit{N}}_{GNSS}$ are the wavelength and ambiguity vector; ${\mathit{C}}_b^n$ represents the rotation matrix from b-frame to n-frame. The IMU center and GNSS receiver center are not matched; therefore, the lever-arm offset ${\mathit{l}}_{GNSS}^b$ is introduced for data fusion [32].

The design matrix ${\mathit{H}}_{k,GNSS}$ can be described as

$${\mathit{H}}_{k,GNSS}=\left[\begin{array}{ccccc}\mathit{A}& 0_{p\times 3}& \mathit{A}\left({\mathit{C}}_b^e{\mathit{l}}_{GNSS}^b\times \right)& 0_{p\times 6}& 0_{p\times 6m}\end{array}\right]$$

(13)

where A denotes the design matrix related to the receiver-satellite geometry; the subscript $p$ stands for the number of GNSS observations.

In harsh urban environments, the GNSS signal is frequently blocked and the ambiguity needs to be frequently augmented to the state vector. In this paper, the ambiguity is pre-obtained by integrating the GNSS observations with the position of INS prediction, which can be illustrated by

$$\left[\begin{array}{c}{\epsilon }_P\\ {\epsilon }_{\varphi }\\ {\epsilon }_{INS}\end{array}\right]=\left[\begin{array}{cc}\mathit{A}& {\mathbf{0}}_{q\times q}\\ \mathit{A}& \mathit{\lambda }\\ I_{3\times 3}& {\mathbf{0}}_{3\times q}\end{array}\right]\left[\begin{array}{c}\delta {\mathit{p}}_r\\ \nabla \Delta N\end{array}\right]-\left[\begin{array}{c}\nabla \Delta {\mathit{P}}_{GNSS}-{\mathit{P}}_{INS}\\ \lambda \nabla \Delta {\varphi }_{GNSS}-{\mathit{P}}_{INS}\\ {\mathbf{0}}_{3\times 1}\end{array}\right]$$

(14)

where $\delta {\mathit{p}}_r$ stands for receiver position error vector. By fusing the high-accuracy INS positions, the accuracy of float ambiguities can be significantly improved [33]. Once the integer ambiguity vector passes the ratio test and bootstrapping success rate [2], the higher-precision carrier phase measurements can be utilized directly in Equation (11).

2.3. Visual Measurement Model

Assuming that there is a static feature point ${\mathit{f}}_{\mathrm{j}}$ observed by camera pose $C_i$, the vision measurement ${\widehat{\mathit{z}}}_i^{(j)}$ can be written as

$${\widehat{\mathit{z}}}_i^{(j)}=\left[\begin{array}{cc}{f}_{\mathit{x}}& 0\\ 0& f_y\end{array}\right]\left[\begin{array}{c}{\hat{X}}_j^{c_i}/{\widehat{Z}}_j^{c_i}\\ {\widehat{Y}}_j^{c_i}/{\widehat{Z}}_j^{c_i}\end{array}\right]+\left[\begin{array}{c}{o}_x\\ o_y\end{array}\right]$$

(15)

where ${\left(\begin{array}{cc}{o}_x& o_y\end{array}\right)}^T$ denotes the measurement noise vector; $f_{\mathit{x}}$ and $f_y$ are the camera focal length; $\left(\begin{array}{ccc}{\hat{X}}_j^{c_i}& {\hat{\mathrm{Y}}}_j^{c_i}& {\hat{\mathrm{Z}}}_j^{c_i}\end{array}\right)$ stands for the feature position vector in the camera frame, which can be written as

$${\widehat{\mathit{p}}}_{{\mathit{f}}_j}^{{\mathit{M}}_i}=\left[\begin{array}{c}{\widehat{X}}_j^{c_i}\\ {\widehat{Y}}_j^{c_i}\\ {\widehat{Z}}_j^{c_i}\end{array}\right]={\mathit{C}}_e^{c_i}\left({\widehat{\mathit{p}}}_{{\mathit{f}}_j}^e-{\widehat{\mathit{p}}}_{c_i}^e\right)$$

(16)

where ${\mathit{C}}_e^{c_i}$ represents the rotation matrix from e-frame to camera-frame; ${\widehat{\mathit{p}}}_{{\mathit{f}}_j}^e$ denotes the feature position vector in the e-frame, which can be obtained by applying the triangulation method [17]; ${\widehat{\mathit{p}}}_{c_i}^e$ is the position vector of the camera pose $C_i$. The residual vector can be represented as

$${\mathit{r}}_i^{(j)}={\mathit{z}}_i^{(j)}-{\widehat{\mathit{z}}}_i^{(j)}$$

(17)

where ${\mathit{z}}_i^{(j)}$ is the coordinate of the pixels measured in the image, which can be found in Figure 1. As shown in Figure 1, $O_{C_i}-X_{C_i}{Y}_{C_i}{Z}_{C_i}$ is the camera frame at i epoch; The $Z_{C_i}$-, $X_{C_i}$-, and $Y_{C_i}$-axes of camera frame point to the front, right, and downward directions; $\pi$ denotes the image plane, which is perpendicular to the principal optical $Z_{C_i}$ and intersects it at the image principal point $\left(C_x,C_y\right)$.

By applying the Taylor expansion, the residual vector can be expressed as

$${\mathit{r}}_i^{(j)}={\mathit{H}}_{X_i}^{(j)}{\mathit{x}}_k+{\mathit{H}}_{{\mathit{f}}_i}^{(j)}\delta {\mathit{p}}_{{\mathit{f}}_j}^e+{\mathit{n}}_i^{(j)}$$

(18)

where ${\mathit{H}}_{{\mathit{X}}_i}^{(j)}$ and ${\mathit{H}}_{{\mathit{f}}_i}^{(j)}$ represent Jacobian matrices of the error state and feature error, which can be expressed as

$${\mathit{H}}_{{\mathit{X}}_i}^{(j)}=\left[\begin{array}{cccccc}{\mathbf{0}}_{2\times 15}& {\mathbf{0}}_{2\times 6}& \cdots & -{\mathit{J}}_i^{(j)}{\mathit{C}}_e^{c_i}\left[\left({\widehat{\mathit{p}}}_{{\mathit{f}}_j}^e-{\widehat{\mathit{p}}}_{c_i}^e\right)\times \right]& -{\mathit{J}}_i^{(j)}{\mathit{C}}_e^{c_i}& \cdots \end{array}\right]$$

(19)

$${\mathit{H}}_{{\mathit{f}}_i}^{(j)}={\mathit{J}}_i^{(j)}{\mathit{C}}_e^{c_i}$$

(20)

with

$${\mathit{J}}_i^{(j)}=\left(1/{\hat{Z}}_j^{C_i}\right)\left[\begin{array}{ccc}{f}_x& 0& -f_x{\hat{X}}_j^{C_i}/{\hat{\mathrm{Z}}}_j^{C_i}\\ 0& f_y& -f_y{\hat{\mathrm{Y}}}_j^{C_i}/{\hat{\mathrm{Z}}}_j^{C_i}\end{array}\right]$$

(21)

where $\left(\begin{array}{cc}{f}_x& f_y\end{array}\right)$ is the focal length of the camera.

Generally, the static feature point ${\mathit{f}}_j$ can be continuously observed by multiple images. Therefore, the residual vector can be rewritten as

$$\underset{{\mathit{r}}^{(j)}}{\underbrace{\left[\begin{array}{c}{\mathit{r}}_1^{(j)}\\ ⋮\\ {\mathit{r}}_m^{(j)}\end{array}\right]}}\approx \underset{{\mathit{H}}_{\mathit{x}}^{(j)}}{\underbrace{\left[\begin{array}{c}{\mathit{H}}_1^{(j)}\\ ⋮\\ {\mathit{H}}_m^{(j)}\end{array}\right]}}{\mathit{x}}_k+\underset{{\mathit{H}}_{\mathit{f}}^{(j)}}{\underbrace{\left[\begin{array}{c}{\mathit{H}}_{f,1}^{(j)}\\ ⋮\\ {\mathit{H}}_m^{(j)}\end{array}\right]}}\delta {\mathit{p}}_{{\mathit{f}}_j}^e+{\mathit{n}}_i^{(j)}$$

(22)

where m denotes the m-th camera state. Obviously, the feature point error $\delta {\mathit{p}}_{{\mathit{f}}_j}^e$ is not in the state error, which doesn’t satisfy the form of the Kalman update process ($\mathit{r}=\mathit{H}{\mathit{x}}_k+\mathit{n}$). Fortunately, the left null space residual matrix ${\mathit{H}}_{{\mathit{f}}_i}^{(j)}$ can be used to solve this problem [28]. Then, it has

$$\begin{array}{l}{\mathit{r}}_{\mathbf{0}}^{\left(j\right)}={\mathit{M}}^T{\mathit{r}}^{\left(j\right)}={\mathit{M}}^T{\mathit{H}}_{\mathit{X}}^{(j)}\delta {\mathit{x}}_k+{\mathit{M}}^T{\mathit{n}}_{\mathbf{0}}^{\left(j\right)}\\ ={\mathit{H}}_{\mathbf{0}}^{(j)}\delta {\mathit{x}}_k+{\mathit{n}}_{\mathbf{0}}^{\left(j\right)}\end{array}$$

(23)

where $\mathit{M}$ stands for the left null space residual matrix of ${\mathit{H}}_{{\mathit{f}}_i}^{(j)}$.

To separate inliers from outliers, the Mahalanobis gating test is applied, which can be expressed as

$${\mathit{r}}_j={\left({\mathit{r}}_{\mathbf{0}}^{\left(j\right)}\right)}^T{\left({\mathit{H}}_{\mathbf{0}}^{(j)}{\mathit{P}}_k{\left({\mathit{H}}_{\mathbf{0}}^{(j)}\right)}^T+{\sigma }^2\mathit{I}\right)}^{-1}{\mathit{r}}_{\mathbf{0}}^{\left(j\right)}$$

(24)

where ${\mathit{P}}_k$ is the filter covariance matrix and ${\sigma }^2$ denotes the variance of the visual measurement. The residuals ${\mathit{r}}_{\mathbf{0}}^{\left(j\right)}$ will be excluded when the terms of ${\mathit{r}}_j$ exceed the threshold calculated by the 95% of the chi-square distribution.

2.4. Robust MSCKF

Based on the above models, the measurement update proceeding the MSCKF can be written as

$$\{\begin{array}{c}{\mathit{x}}_k={\Phi }_{k,k-1}{x}_{k-1}+K_k\left({\mathit{z}}_k-{\mathit{H}}_k{\Phi }_{k,k-1}{x}_{k-1}\right)\\ {\mathit{P}}_k=\left(\mathbf{I}-K_k{\mathit{H}}_k\right)\left({\Phi }_{k,k-1}{\mathit{P}}_{k-1}{\Phi }_{k,k-1}^T+Q_{k-1}\right){\left(\mathbf{I}-K_k{\mathit{H}}_k\right)}^T+K_k{R}_k{K}_k^T\\ K_k=\left({\Phi }_{k,k-1}{\mathit{P}}_{k-1}{\Phi }_{k,k-1}^T+Q_{k-1}\right){\mathit{H}}_k^T{\left({\mathit{H}}_k\left({\Phi }_{k,k-1}{\mathit{P}}_{k-1}{\Phi }_{k,k-1}^T\right){\mathit{H}}_k^T+R_k\right)}^{-1}\end{array}$$

(25)

where ${\mathit{P}}_k$ denotes the variance-covariance (VC) matrix of the error state; $K_k$ and $\mathbf{I}$ are the gain matrix and unit matrix; $R_k$ represents the priori VC matrix of the measurement noise; ${\mathit{H}}_k$ is the design matrix, which can be expressed as ${\mathit{H}}_k={\left[\begin{array}{cc}{\mathit{H}}_{k,GNSS}& {\mathit{H}}_{\mathbf{0}}^{(j)}\end{array}\right]}^T$; ${\Phi }_{k,k-1}$ denotes the transition matrix of error state, which can be found in Equation (8); $Q_{k-1}$ is the covariance matrix associated with state noise vector ${\mu }_{k-1}$ at epoch k, it is initialized by the priori parameter values of IMU, including the velocity random walks (VRW), angle random walks (ARW), in-run instability of gyroscope, and the correlation time of the in-run bias.

To eliminate the effect of the measurement outliers, the IGG-III model [34] is utilized, which can be expressed as

$${\tilde{R}}_k=\beta R_k$$

(26)

with

$${\beta }_{uu}=\{\begin{array}{c}1,\left|{\tilde{\mathit{v}}}_{k,u}\right|\le k_0\\ \frac{{\tilde{\mathit{v}}}_{k,u}}{k_0}\times \left(\frac{k_1-k_0}{k_1-{\tilde{\mathit{v}}}_{k,u}}\right)\\ \infty ,\left|{\tilde{\mathit{v}}}_{k,u}\right|\ge k_1\end{array},k_0\le \left|{\tilde{\mathit{v}}}_{k,u}\right|\le k_1$$

(27)

where ${\tilde{R}}_k$ denotes the VC matrix of the measurement noise after applying the robust factor $\beta$; $k_0$ and $k_1$ denote the thresholds, with $k_0$ being set to 2.0~3.0 and $k_1$ being set to 4.5~8.5; The u-th standardized residual ${\tilde{\mathit{v}}}_{k,u}$ at epoch k is calculated by the following formula:

$${\tilde{\mathit{v}}}_{k,u}=\frac{{\left({\mathit{z}}_k-{\mathit{H}}_k{\mathit{x}}_k\right)}_{uu}}{{\sqrt{\left({\mathit{H}}_k{\mathit{P}}_{k,k-1}{\mathit{H}}_k^T+R_k\right)}}_{uu}}$$

(28)

where ${\mathit{P}}_{k,k-1}$ is the predicted VC matrix of the state parameters.

Finally, the new form of measurement update can be obtained by substituting Equations (24)~(26) into Equation (23)

$$\{\begin{array}{c}{\mathit{x}}_k={\Phi }_{k,k-1}{\mathit{x}}_{k-1}+K_k\left({\mathit{z}}_k-{\mathit{H}}_k{\Phi }_{k,k-1}{\mathit{x}}_{k-1}\right)\\ {\mathit{P}}_k=\left(\mathbf{I}-K_k{\mathit{H}}_k\right)\left({\Phi }_{k,k-1}{\mathit{P}}_{k-1}{\Phi }_{k,k-1}^T+Q_{k-1}\right){\left(\mathbf{I}-K_k{\mathit{H}}_k\right)}^T+K_k{\tilde{R}}_k{K}_k^T\\ K_k=\left({\Phi }_{k,k-1}{\mathit{P}}_{k-1}{\Phi }_{k,k-1}^T+Q_{k-1}\right){\mathit{H}}_k^T{\left({\mathit{H}}_k\left({\Phi }_{k,k-1}{\mathit{P}}_{k-1}{\Phi }_{k,k-1}^T\right){\mathit{H}}_k^T+{\tilde{R}}_k\right)}^{-1}\end{array}$$

(29)

2.5. Sensors-Aided Cycle Slip Detection

Besides, small cycle-slip detection is also essential for the performance improvement of the GNSS SF-RTK/INS/Vision (RIV) tight integration model. To avoid the impact of the small cycle slip, the sensors-predicted carrier phase increment method is utilized in this contribution. The main idea of this method is to compare the sensors-predicted carrier phase increment with the double-differenced (DD) carrier phase increment. According to [35], the sensors-predicted carrier phase increment can be written as

$$\delta {\phi }_{INS}^{t,t-1}=\frac{1}{\lambda }\left(\nabla \Delta {\rho }_{INS}^t-\nabla \Delta {\rho }_{INS}^{t-1}\right)$$

(30)

where $\nabla \Delta {\rho }_{INS}^t$ and $\nabla \Delta {\rho }_{INS}^{t-1}$ denote the sensors-predicted DD range at t and t − 1 epochs, respectively. Similarly, the DD carrier phase increment from receivers can be calculated by

$$\delta {\phi }_{GNSS}^{t,t-1}=\frac{1}{\lambda }\left(\nabla \Delta {\phi }_{GNSS}^t-\nabla \Delta {\phi }_{GNSS}^{t-1}\right)$$

(31)

where $\nabla \Delta {\phi }_{GNSS}^t$ and $\nabla \Delta {\phi }_{GNSS}^{t-1}$ denote the DD carrier phase observations from receivers at t and t − 1 epochs, respectively. By differencing Equations (23) and (24), the detection factor can be obtained by

$$DT=\delta {\phi }_{GNSS}^{t,t-1}-\delta {\phi }_{INS}^{t,t-1}$$

(32)

The threshold of the detection factor can be chosen empirically. In this paper, the threshold is set to 0.4 according to the results in [35].

2.6. Algorithm Summary

The algorithm implementation of the presented RTK/INS/Vision integration model is presented in Figure 2. In such a system, the visual data, single-frequency GNSS observations, and IMU measurements are fused in MSCKF. The position and attitude calculated by RTK solutions are used to initialize the system [36]. After that, the raw angular rate and specific force are processed by the INS mechanization algorithm to provide state information. Once the GNSS time is synchronized with IMU, the DD pseudo-range and DD phase-carrier will be integrated with the INS-predicted range to update the system state. To avoid the impact of outliers, the IGG-III model [34] and sensors-aided cycle-slip detection method [35] are used in this stage. Then, the LAMBDA algorithm [37,38] is performed to re-update the state by using high-accuracy phase-carrier information. For the visual part, the feature points are extracted by the fast corner detector and tracked by Kanade–Lucas (KL) method [39,40]. In the front-end, the random sample consensus (RANSAC) method [41] is performed to resist feature outliers. Once the new image arrives, the current IMU pose and the corresponding covariance matrix can be augmented. If the feature points disappear or the number of state parameters in the sliding window reaches the maximum, the visual measurement after passing the chi-square test [42] will be applied to update the state parameters. Finally, the estimated IMU sensor errors will be fed back to compensate the IMU raw measurements in the next epoch.

3. Experiments and Data Processing Schemes

To validate the presented algorithm, a wheeled robot test with an average velocity of 1.5 m/s was carried out at Wuhan University on 12 December 2022. The test trajectory and typical scenarios are shown in Figure 3 and Figure 4, respectively. During the test, two different grades of IMU are used to collect IMU data. The MEMS-grade IMU (ADIS16465) is processed and analyzed to evaluate the performance of the presented system. The specifications of the MEMS-IMU can be found in

Table 1. Specifications of the MEMS-IMU.

Bias		Random Walk
$\mathbf{Gyro}.(°/\mathbf{h})$	Acce. (mGal)	$\mathbf{Angular}(°/\sqrt{\mathbf{h}})$	$\mathbf{Velocity}(\mathbf{m}/\mathbf{s}/\sqrt{\mathbf{h}})$
36	3000	3.17	2.7

. The employed ground truth system is a high-accuracy position and orientation system (POS), using the GNSS RTK and a navigation-grade IMU. The ground truth (0.02 m for position and 0.01 deg for attitude) was generated by a post-processing GNSS/INS integration software [24]. A global shutter camera (Mako-G131) and a GNSS receiver (OEM-718D) are used to collect the image data and GNSS observations. In this test, the timestamps of the three sensors are united to GPS time, which is realized by hardware triggering. The lever-arm offsets and intrinsic-extrinsic parameters are accurately synchronized before the test.

The test data (GitHub-i2Nav-WHU/IC-GVINS: A robust, real-time, INS-centric GNSS-visual-inertial navigation system) are processed by (1) Vision/IMU tight integration mode based on MSCKF (MSCKF VIO), (2) GPS + BDS single-frequency RTK/INS tight integration mode based on EKF (SF-RI), (3) GPS + BDS single-frequency RTK/INS/vision tight integration mode based on MSCKF (SF-RIV), and (4) GPS + BDS dual-frequency RTK/INS/vision tight integration mode based on MSCKF (DF-RIV), respectively. The specific differences of these schemes are listed in

Table 2. The specific differences of the schemes.

Mode	Visual Data	IMU Data	Dual Frequency GNSS Data	Single Frequency GNSS Data
MSCKF VIO	Δ	Δ
SF-RI		Δ		Δ
DF-RIV	Δ	Δ	Δ
SF-RIV	Δ	Δ		Δ

. In the GNSS data processing, the atmospheric delay can be neglected due to the baseline length being less than 2 km [43]. The ratio test and prior success rate are set to 3.0 and 0.99 [33]. To avoid the influences of GNSS and image observation outliers, the IGG-III model, random sample consensus (RANSAC), and chi-square test were performed.

To assess the deviations between the estimated values and reference values and error distribution, the root mean square error (RMSE) and cumulative distribution function (CDF) defined in Equation (33) is used,

$$\{\begin{array}{l}RMSE=\sqrt{\frac{1}{t}{\displaystyle \sum _{i=1}^t{({\mathrm{y}}_i-{\widehat{\mathrm{y}}}_i)}^2}}\\ CDF\left(F\right)=P\left(F\le f\right)\end{array}$$

(33)

where ${\mathrm{y}}_i$ denotes the estimated value; ${\widehat{\mathrm{y}}}_i$ is the reference value; $\mathrm{P}(\cdot )$ denotes the probability value; $F$ and $f$ represent the random variable and independent variable, respectively.

Satellite availability is essential to positioning solutions. As shown in Figure 4, there are buildings and dense trees during the test, which would significantly impact the GNSS signal quality [44,45]. Figure 5 shows the tracked satellites. It can be seen that only the GPS and BDS satellites are tracked during the test. Due to the tall buildings and dense tree lines, the satellite geometry condition is poor, which will be detrimental to position dilution of precision (PDOP) and positioning accuracy [2,6,35].

Figure 6 describes the number of tracked satellites with a cut-off elevation angle of 10°. It can be seen that the satellite signals are interrupted frequently under obstructing roads. The number of satellites on average is 2.90, and only 34.48% of epochs observe more than four satellites. Such poor observability is also reflected in the PDOP value (as shown in Figure 6b). According to the statistics, the average PDOP is 10.08 and reaches 69.60 in extreme conditions. All the factors mean the availability of GNSS is limited, which is a challenge to the RTK positioning.

4. Discussions

The performance of the proposed system in terms of sensors-aided cycle slip detection, position, attitude, and run-time are presented in detail in this section.

4.1. Sensors-Aided Cycle Slip Detection

To verify the capability of cycle slip detection with sensors aids, one cycle slip on different satellites is simulated. Figure 7 shows the sensors-aided cycle-slip detection results of different satellites. We can see that all the simulated cycle slips can be detected correctly, and the corresponding detection terms are close to one. This indicates that the sensors-aided cycle-slip detection method has the advantage of detecting small cycle-slip, which would bring benefits for positioning while using the single-frequency observations. Figure 8 describes the time series of the detected cycle-slips value during the measured test. It can be seen that most of the detection terms are below the threshold. However, frequent cycle slips still happen, which reflects the complexity of test environments.

4.2. Enhancements in Positioning Accuracy

To illustrate the positioning accuracy benefits from the provided integration model, the position differences between the solutions calculated by the four schemes and the reference values are analyzed. Figure 9 shows the time series of these positioning errors. It can be found that the positioning accuracy of SF RTK has a strong correlation with signal reception. The statistics results illustrate that the availability of SF RTK is only 43.79% during this test.

Table 3. Position RMSE of different modes.

Modes	SF RTK	SF RI	MSCKF VIO	DF RIV	SF RIV
North (m)	1.84	22.72	4.01	1.33	0.88
East (m)	1.33	49.91	2.67	0.99	0.88
Down (m)	5.09	15.08	2.26	1.60	0.65

lists the position RMSE of the different modes. Accordingly, the root mean square error (RMSE) of SF RTK are 1.84 m, 1.33 m, and 5.09 m in the north, east, and vertical components, respectively. Obviously, it’s difficult to adopt RTK solutions to provide continuous and high-accuracy positioning results in such complex environments. While SF RTK is integrated tightly with INS, the continuity and accuracy are further improved visibly. Accordingly, the position RMSE of SF RTK/INS tight integration mode during these SF RTK available periods are 0.61 m, 0.86 m, and 2.31 m in the three directions, with improvement percentages of 66.85%, 35.34%, and 54.62%, compared to the SF RTK-only mode. This indicates that the SF RTK/INS integrated model can provide higher position results. However, due to the frequent GNSS outages, the SF RTK/INS tight integration mode degenerates to the INS-only mode. During these GNSS-outage periods, the maximum position drifts increase rapidly to 138.83 m, 342.92 m, and 24.27 m in the three directions. Different from the vulnerable GNSS signals, the visual information is continuous and abundant [28]. The time series of the position errors of the MSCKF VIO mode presents a significant upgrade in position accuracy. The corresponding RMSE in the three directions is 4.01 m, 2.67 m, and 2.26 m, respectively. Visibly, the position drifts of INS are constrained. Accordingly, the maximum drifts of VIO mode are 7.88 m, 3.46 m, and 3.44 m in north, east, and vertical components, with position drift reductions of 94.32%, 98.99%, and 85.83%, respectively. It proves that vision information can significantly limit the impact of the accumulation of IMU errors on position, especially in GNSS-denied areas.

Although the VIO system has shown that it can limit the position drifts, the time-varying IMU sensor errors are still accumulated due to the lack of global measurements. With the fusion of single-frequency GNSS data, a global constraint on IMU errors will be formed. To illustrate the benefits of single-frequency GNSS data, the time series of position differences of dual-frequency and single-frequency GNSS data augmented VIO are provided in Figure 10. Accordingly, the position RMSE of DF RIV mode in the north, east, and vertical components are 1.33 m, 0.99 m, and 1.60 m, with the enhancements of 66.75%, 56.02%, and 29.06% compared to that of VIO mode. The maximum position errors in the three directions are 1.86 m, 0.80 m, and 2.98 m, with reductions of 76.40%, 76.88%, and 13.37% compared to that of VIO. For the SF RIV mode, such improvements are more obvious. The position RMSE values are reduced to 0.88 m, 0.88 m, and 0.65 m in the three directions, with improvement percentages of 77.95%, 66.96%, and 71.07% compared to that of VIO. The maximum position errors are 2.56 m, 1.35 m, and 0.40 m, with about 67.51%, 60.98%, and 88.37% extra reductions in the three components, by comparing with that of the VIO. The results indicate that the accuracy and continuity of VIO can be improved by the RTK/INS/Vision integrated system noticeably by either dual-frequency or single-frequency GNSS data. This is because GNSS observations can provide global constraints to the VIO system. It can significantly reduce the impact of IMU drifts in position estimation. It should be noted that the enhancements based on single-frequency data are higher than those based on dual-frequency data because more available satellites can be available while adopting the single-frequency GNSS data, which leads to stronger global constraints. According to the statistics, the average number of satellites for single-frequency observations is 2.90, which presents about 31.72% improvements compared to that when applying dual-frequency data (1.98).

The 3D trajectories of different positioning modes are shown in Figure 11. Due to the frequent GNSS outages, the trajectory calculated by SF RTK/INS tight integration mode is a serious deviation from the truth trajectory in GNSS gaps. While applying the Visual/INS tight integration mode, the result is closer to the truth trajectory compared to that of SF RTK/INS tight integration mode. This is because the pose and IMU errors can be consistently estimated by integrating external continuous visual information (as shown in Equation (22)). After fusing the dual-frequency GNSS observations, the DF RTK/INS/vision tight integration system can provide more accurate position results, especially in the vertical direction. Such improvements are more obvious while using the GNSS SF data due to more observations. To further illustrate the positioning performance of the four positioning modes, the position errors of different modes in the horizontal direction are projected into the trajectories (Figure 12). We set the color of the trajectory to change from green to yellow when the position errors increased from 0.0 m to 8.5 m. The specific cumulative distribution function (CDF) results of position differences of different modes are shown in Figure 13. Visibly, the trajectory calculated by SF RIV mode depicts deeper green colors, which means a higher-accuracy position. According to the statistics, the horizontal errors at 68% of the CDF are 3.45 m, 5.20 m, 2.12 m, and 1.57 m for the modes of SF RI, VIO, DF RIV, and SF RIV, respectively.

4.3. Contributions to Attitude Determination

For land vehicles, the accuracy of attitude is even more crucial. Therefore, the impact of the proposed model on attitude determination has also been investigated. Figure 14 shows the time series and CDF of attitude differences for the modes of MSCKF VIO, SF RI, DF RIV, and SF RIV, respectively. Due to the frequent partial and complete GNSS outages, the attitudes of SF RI mode are rapidly drifting. It can be noted that the maximum heading error is up to 8°, which is worse than roll and pitch directions. This is because of the weak observability of the gyroscope in the vertical component under the GNSS-denied area [33]. While applying the VIO mode, the phenomenon of attitude drifts disappears significantly.

Table 4. Attitude RMSE of different modes.

Modes	SF RI	MSCKF VIO	DF RIV	SF RIV
Roll (°)	0.35	0.11	0.12	0.11
Pitch (°)	0.36	0.24	0.24	0.28
Heading (°)	4.57	1.79	1.04	1.04

lists the attitude RMSE of different data processing modes. For VIO mode, the attitude RMSE in three directions are 0.11°, 0.24°, and 1.79°, which presents about 68.57%, 33.33%, and 60.83% enhancements compared to that of SF RI mode. It proves that the visual information can enhance the observability of attitudes and IMU sensor errors (as shown in Equation (18)). After integrating the GNSS DF/SF observations, the attitudes of RMSE improve to 0.12°/0.11°, 0.24°/0.28°, and 1.04°/1.04°. The improvements are mainly reflected in the heading direction (41.90%), which is due to the enhancement of the heading observability. As shown in Figure 14b, the roll attitude errors at 68% of the CDF are 0.248°, 0.114°, 0.118°, and 0.108° for the schemes of SF RI, VIO, DF RIV, and SF RIV, and the pitch attitude error at 68% of the CDF are 0.174°, 0.250°, 0.275°, and 0.255°. The heading attitude errors at such a percentage of the CDF are 5.268°, 1.985°, 1.156°, and 1.142° for the schemes of SF RI, VIO, DF RIV, and SF RIV. The above statistics indicate that the accuracy of attitude estimation, especially in the heading direction, can be further improved while fusing the original GNSS observations and camera images.

4.4. Analysis of Running Time

The data are processed on the desktop with an 11th Gen Intel Core i7-11700K CPU and 32 GB RAM.

Table 5. The average running time of the presented model.

Types	Time (ms)
Feature detecting and tracking	26.42
Visual measurement update	59.24
GNSS measurement processing	1.36
Total	87.02

lists the average running time of the major time-consuming parts in the presented model, which can be summarized as (1) feature detecting and tracking: fast corner detection and KLT tracking proceedings; (2) visual measurement update: the selection of the feature points in measurement update, the calculation of the selected feature point position, and the state update process of integrating the visual measurements; (3) GNSS measurement processing: the construction of DD observations, the float/integer ambiguity pre-obtained by applying INS-aided ambiguity resolution methods, and the state update of fusing the GNSS measurements. According to the statistics, the total running time on average is 87.02 ms. Such factors prove that the presented model can be effectively used in real-time applications [24,46].

5. Conclusions

In this contribution, we present a low-cost GNSS RTK/INS/vision tight integration model based on the MSCKF method to achieve high-accuracy and real-time positioning results. A set of wheeled robot data under satellite signal-blocked conditions are utilized to validate the performance in terms of sensors-aided cycle slip detection, position, attitude, and run time. Results illustrate that the sensors-aided cycle-slip detection method has the advantage of detecting small cycle-slip while adopting single-frequency GNSS observations. The position accuracy in the north, east, and vertical directions are upgraded to 0.88 m, 0.88 m, and 0.64 m, with improvements of 77.95%/33.83%, 66.96%/11.11%, and 71.07%/59.38% compared to that of the MSCKF VIO and dual-frequency RTK/INS/vision modes, while applying the single-frequency GNSS/INS/vision tight integration model. For the attitudes, improvement is mainly reflected in the heading component due to the observability enhancements on the heading angle and gyroscope in the vertical direction. The average running time significantly proves that the presented model has the potential for real-time applications.