A Tightly Coupled Visual-Inertial GNSS State Estimator Based on Point-Line Feature

Abstract

Visual-inertial odometry (VIO) is known to suffer from drifting and can only provide local coordinates. In this paper, we propose a tightly coupled GNSS-VIO system based on point-line features for robust and drift-free state estimation. Feature-based methods are not robust in complex areas such as weak or repeated textures. To deal with this problem, line features with more environmental structure information can be extracted. In addition, to eliminate the accumulated drift of VIO, we tightly fused the GNSS measurement with visual and inertial information. The GNSS pseudorange measurements are real-time and unambiguous but experience large errors. The GNSS carrier phase measurements can achieve centimeter-level positioning accuracy, but the solution to the whole-cycle ambiguity is complex and time-consuming, which degrades the real-time performance of a state estimator. To combine the advantages of the two measurements, we use the carrier phase smoothed pseudorange instead of pseudorange to perform state estimation. Furthermore, the existence of the GNSS receiver and IMU also makes the extrinsic parameter calibration crucial. Our proposed system can calibrate the extrinsic translation parameter between the GNSS receiver and IMU in real-time. Finally, we show that the states represented in the ECEF frame are fully observable, and the tightly coupled GNSS-VIO state estimator is consistent. We conducted experiments on public datasets. The experimental results demonstrate that the positioning precision of our system is improved and the system is robust and real-time.

1. Introduction

Localization plays an important role in many applications such as robotics, unmanned driving, and unmanned aerial vehicles (UAVs). The fusion of information from multiple sensors for localization is the current mainstream method. The sensor fusion approaches have been widely studied for decades and can be divided into two streams: loosely coupled and tightly coupled approaches. Loosely coupled approaches [1,2] process the measurements of each sensor individually and then fuse all the results to obtain the final results. Tightly coupled approaches [3,4] combine measurements from all sensors and then use an estimator to process these measurements to obtain the final results. In general, the accuracy of tightly coupled approaches will be higher than that of loosely coupled approaches.

Visual odometry (VO) has received more attention due to the low price of cameras. Davison et al. [5] introduced MonoSLAM, which is the first monocular visual SLAM system that extracts the Shi-Tomasi corner [6]. MonoSLAM actively searches for matching point features in projected ellipses. However, because of the high computational complexity of MonoSLAM, it can only handle some small scenes. Klein et al. [7] proposed PTAM, which is the first optimization-based monocular visual SLAM system that executes feature tracking and mapping as two independent tasks in parallel in two threads.

Because PTAM is designed for small-scale AR scenes, it also has some disadvantages. For example, it can only handle small-scale scenes, and the tracked features are easy to lose. A milestone work after PTAM is ORB-SLAM [8] and ORB-SLAM2 [9]. ORB-SLAM improves PTAM by detecting and tracking ORB features and adding loop closure. These improvements greatly improve the positioning precision of VO. However, ORB-SLAM can only build sparse point cloud maps and cannot track keyframes during pure rotation, which also limits its application.

In addition, when encountering environments with repeated textures, point features cannot express environmental structure information well. On the contrary, line features contain more information related to the environmental structure and can overcome the interference caused by repeated textures. Therefore, point-line feature fusion can ensure that the VO system is more robust. He Yijia et al. [10] proposed PL-VIO, which is a VIO system based on point-line features. It uses the LSD line segment extraction algorithm [11] from OpenCV [12] to detect line features. However, LSD becomes the bottleneck for real-time performance due to its high computational cost [13], which causes the performance of PL-VIO to be seriously affected. To improve the speed and positioning precision of PL-VIO, Fu Qiang et al. [14] proposed PL-VINS, which is also a VIO framework based on point and line features. PL-VINS designs a modified LSD algorithm by studying hidden parameter tuning and length rejection strategy. The modified LSD runs at least three times faster than the LSD. PLF-VINS [15] is another VIO system based on point-line features, which introduces two methods for fusing point and line features. The first method is to use the positional similarity of point-line features to search for the relationship between point and line features. The second method is to fuse 3D parallel lines. The residuals formed by the two methods are then added to the VIO system. The results of PLF-VINS show that its positioning precision is greatly improved compared to some classic SLAM systems, such as OKVIS [4] and VINS-Mono [16].

Line features can improve the positioning precision of the VO system. However, in weak textures or dark scenes, the system still cannot extract a sufficient number of point and line features, which can cause large positioning errors. To overcome the shortcoming, fusing IMU measurements with images is a very effective method. MSCKF [17,18] is a VIO system based on EKF, which adds camera poses to the states of the system. When cameras observe landmarks, constraints can be formed between camera poses. The system is then updated by an observation model derived from geometric constraints. Since the number of camera poses is much smaller than the number of landmarks, this greatly reduces the time complexity of the system. VINS-Mono [16] is a tightly coupled, nonlinear optimization-based method that can obtain high-precision positioning results. Due to the existence of the loop closure and 4-DoF pose graph optimization, even if the system runs in large-scale scenes, it can still obtain accurate positioning results. In addition, to improve the performance of ORB-SLAM and eliminate the errors of ORB-SLAM, ORB-SLAM3 [19] adds IMU measurements based on ORB-SLAM. ORB-SLAM3 is more robust and has higher positioning precision compared with ORB-SLAM.

The VIO system is more robust than the VO system. Nevertheless, the VIO system has four unobservable directions, namely x, y, z, and yaw, which lead to the accumulated drift of VIO. To eliminate the accumulated drift, an effective approach is to combine VIO with GNSS measurements. Lee et al. [20] demonstrated that the GPS-aided VIO system is fully observable in the ENU frame. GVINS [21] is a tightly coupled GNSS-VIO state estimator that combines VIO with GNSS pseudorange and Doppler frequency shift measurements, which achieves high positioning accuracy and is a state-of-the-art method. Li Xingxing et al. [22] introduced a semi-tightly coupled framework based on GNSS Precise Point Positioning (PPP) and stereo VINS. The system can use S-VINS to return accurately predicted positions in GNSS-unfriendly areas. Liu Jinxu et al. [23] also proposed a tightly coupled GNSS-VIO state estimator that fuses GNSS raw measurements with VIO. It drops all GNSS measurements in GNSS-degradation scenes, which limits its positioning precision.

However, there are many challenges in fusing the information from multiple sensors. First, the VIO system cannot extract enough point features in areas with repeated textures. Second, GNSS Single Point Positioning (SPP) uses pseudorange measurement; it can only achieve meter-level positioning accuracy. Therefore, if the pseudorange measurement and VIO are directly fused, the positioning accuracy is not greatly improved. Finally, since the GNSS receiver and IMU are fixed in different spatial positions, it is necessary to estimate the extrinsic parameter between them.

In response to the above problems, this paper proposes a new tightly coupled GNSS-VIO system. Our proposed system is drift-free and can provide global coordinates. The main contributions of the text are as follows:

• To obtain environmental structure information and deal with environments with repeated textures, we extracted line features based on point features.
• To combine the merits of the pseudorange and carrier phase measurements, we used the carrier phase smoothed pseudorange instead of the pseudorange measurement, which can make the GNSS-VIO system run in real-time and improve the positioning accuracy.
• We demonstrate that the states represented in the ECEF frame are fully observable, and the tightly coupled GNSS-VIO state estimator is consistent.

The rest of this paper is organized as follows: Section 2 introduces the implementation method of GNSS-VIO in detail, including line features, GNSS raw measurements processing, and observability analysis. Section 3 summarizes the detailed structure of our system. Section 4 conducts experiments on public datasets. Finally, conclusions are given in Section 5.

2. Methods

2.1. Frames and Notations

The frames involved in our system consist of:

• Sensor Frame: In our system, ${(\cdot )}^c$, ${(\cdot )}^b$, and ${(\cdot )}^r$ denote the camera frame, the body frame, and the GNSS receiver frame respectively.
• Local World Frame: The origin of the local world frame ${(\cdot )}^w$ is the position where the VIO system starts running, and the z-axis is gravity aligned as illustrated in Figure 1.
• ENU Frame: The ENU frame is also called the East–North–Up frame. The x-, y-, and z-axis of the ENU frame point to the east, north, and up directions, respectively. In our system, the ENU frame is at the same origin as the local world, and the z-axis of the two frames is aligned (Figure 1). We use ${(\cdot )}^N$ to represent the ENU frame.
• Geodetic Frame: As shown in Figure 1, geodetic coordinate ${(\cdot )}^G$ of a point p is represented by geodetic longitude L, geodetic latitude B, and geodetic height H. The geodetic longitude L of p is the angle between the reference meridian and another meridian that passes through p. The geodetic latitude B of p is the angle between the ellipsoid normal vector that passes through p and the projection of the ellipsoid normal vector into the equatorial plane. The geodetic height H of p is the minimum distance between p and the reference ellipsoid.
• ECEF Frame: The Earth-Centered, Earth-Fixed (ECEF) frame ${(\cdot )}^E$ is fixed to Earth. As depicted in Figure 1, the origin of the ECEF frame is at the center of mass of Earth. The x-axis points to the intersection of the Equator and the Prime Meridian. The z-axis is perpendicular to the equatorial plane in the direction of the North Pole. The y-axis is chosen to form a right-handed coordinate system with the x- and z-axis.

In this paper, ${\mathbf{R}}_b^w$ and ${\mathbf{p}}_b^w$ represent the rotation and translation of the body frame with respect to the local world frame, and ${\mathbf{q}}_b^w$ is the corresponding quaternion form of the rotation ${\mathbf{R}}_b^w$. ${\mathbf{v}}_b^w$, ${\mathbf{b}}_a$, and ${\mathbf{b}}_g$ denote the velocity of the origin of the body frame measured in the local world frame, accelerometer bias, and gyroscope bias, respectively. ${\mathbf{p}}_c^b$ and ${\mathbf{q}}_c^b$ stand for the extrinsic parameters between the camera and IMU. $\delta {\mathbf{t}}_r$ and $\delta {\dot{\mathbf{t}}}_r$ represent the receiver clock error and receiver clock drifting rate, respectively. $\phi$ is the yaw between the local world frame and the ENU frame. ${\mathbf{R}}_w^N$ is the rotation matrix of the local world frame with respect to the ENU frame. ${\mathbf{p}}_N^E$ denotes the translation of the ENU frame with respect to the ECEF frame. ${\mathbf{p}}_r^b$ is the extrinsic translation parameter between the GNSS receiver and IMU. ${[\cdot ]}_{\times }$ represents the skew-symmetric matrix of a 3D vector.

2.2. Line Feature

2.2.1. Plücker Coordinates

In our system, we described a spatial line with the Plücker Coordinates. Given a spatial line $L_w\in {\pi }_w$, its Plücker Coordinates are represented by $L_w={({\mathbf{n}}_w^{\mathrm{T}},{\mathbf{d}}_w^{\mathrm{T}})}^{\mathrm{T}}$, where ${\mathbf{n}}_w\in {ℝ}^3$ is the normal vector of the plane determined by $L_w$, and the origin of ${\pi }_w$ and ${\mathbf{d}}_w\in {ℝ}^3$ is the line direction vector. We can transform $L_w$ in the local world frame to $L_c$ in the camera frame by:

$$L_c=\left[\begin{array}{cc}{\mathbf{R}}_w^c& {[{\mathbf{p}}_w^c]}_{\times }{\mathbf{R}}_w^c\\ \mathbf{0}& {\mathbf{R}}_w^c\end{array}\right]L_w={\mathbf{T}}_w^c{L}_w,$$

(1)

2.2.2. Line Feature Triangulation

Assuming that ${\mathcal{L}}_w$ is observed by camera $c_i$ and camera $c_j$ in the normalized image plane as ${\mathit{z}}_{{\mathcal{L}}_w}^{c_i}$ and ${\mathit{z}}_{{\mathcal{L}}_w}^{c_j}$, the two line segments can be denoted by two endpoints as shown in Figure 2. The two endpoints of ${\mathit{z}}_{{\mathcal{L}}_w}^{c_i}$ are ${\mathbf{s}}_{{\mathcal{L}}_w}^{c_i}={\left[u_s^{c_i},v_s^{c_i},1\right]}^{\mathrm{T}}$ and ${\mathbf{e}}_{{\mathcal{L}}_w}^{c_i}={\left[u_e^{c_i},v_e^{c_i},1\right]}^{\mathrm{T}}$, and the two endpoints of ${\mathit{z}}_{{\mathcal{L}}_w}^{c_j}$ are ${\mathbf{s}}_{{\mathcal{L}}_w}^{c_j}={\left[u_s^{c_j},v_s^{c_j},1\right]}^{\mathrm{T}}$ and ${\mathbf{e}}_{{\mathcal{L}}_w}^{c_j}={\left[u_e^{c_j},v_e^{c_j},1\right]}^{\mathrm{T}}$.

A 3D plane $\pi$ can be modeled as:

$${\mathit{\pi }}_{x}x+{\mathit{\pi }}_{y}y+{\mathit{\pi }}_{z}z+{\mathit{\pi }}_w=0,$$

(2)

where ${\mathbf{n}}_{\pi }={[{\pi }_x,{\pi }_y,{\pi }_z]}^{\mathrm{T}}$ is the normal vector of $\pi$. For a point $p_0={\left[x_0,y_0,z_0\right]}^{\mathrm{T}}$ on the plane, we can obtain:

$${\mathit{\pi }}_w=-\left({\mathit{\pi }}_x{x}_0+{\mathit{\pi }}_y{y}_0+{\mathit{\pi }}_z{z}_0\right)=-{\mathbf{n}}_{\pi }^{\mathrm{T}}\cdot p_0,$$

(3)

According to Equations (2) and (3), we can obtain the plane ${\mathit{\pi }}_i=\left({\mathbf{n}}_{{\pi }_i},{\mathit{\pi }}_{w_i}\right)$ and ${\mathit{\pi }}_j=\left({\mathbf{n}}_{{\pi }_j},{\mathit{\pi }}_{w_j}\right)$. As shown in Figure 2, the normal vector of ${\mathit{\pi }}_i$ is ${\mathbf{n}}_{{\pi }_i}={\left[{\mathbf{s}}_{{\mathcal{L}}_w}^{c_i}\right]}_{\times }{\mathbf{e}}_{{\mathcal{L}}_w}^{c_i}$, and ${\mathit{\pi }}_{w_i}$ can be computed by the optical center $c_i={\left[0,0,0\right]}^{\mathrm{T}}$ as ${\mathit{\pi }}_{w_i}=-{\mathbf{n}}_{{\pi }_i}^{\mathrm{T}}\cdot c_0=0$. To obtain ${\mathit{\pi }}_j$, we need to transform the two endpoints ${\mathbf{s}}_{{\mathcal{L}}_w}^{c_j}$ and ${\mathbf{e}}_{{\mathcal{L}}_w}^{c_j}$ to the camera frame $c_i$. Therefore, the corresponding reprojected endpoints are ${\tilde{\mathbf{s}}}_{{\mathcal{L}}_w}^{c_j}={\mathbf{R}}_{c_j}^{c_i}{\mathbf{s}}_{{\mathcal{L}}_w}^{c_j}+{\mathbf{p}}_{c_j}^{c_i}$ and ${\tilde{\mathbf{e}}}_{{\mathcal{L}}_w}^{c_j}={\mathbf{R}}_{c_j}^{c_i}{\mathbf{e}}_{{\mathcal{L}}_w}^{c_j}+{\mathbf{p}}_{c_j}^{c_i}$, where ${\mathbf{R}}_{c_j}^{c_i}$ and ${\mathbf{p}}_{c_j}^{c_i}$ can be obtained by visual-inertial alignment. Similarly, the normal vector of ${\mathit{\pi }}_j$ is ${\mathbf{n}}_{{\pi }_j}={\left[{\tilde{\mathbf{s}}}_{{\mathcal{L}}_w}^{c_j}\right]}_{\times }{\tilde{\mathbf{e}}}_{{\mathcal{L}}_w}^{c_j}$, and ${\mathit{\pi }}_{w_j}$ can be calculated by the translation ${\mathbf{p}}_{c_j}^{c_i}$ from the camera frame $c_j$ to the camera frame $c_i$ as ${\mathit{\pi }}_{w_j}=-{\mathbf{n}}_{{\pi }_j}^{\mathrm{T}}\cdot {\mathbf{p}}_{c_j}^{c_i}$.

After ${\mathit{\pi }}_i$ and ${\mathit{\pi }}_j$ are computed, we can obtain the Plücker Coordinates of ${\mathcal{L}}_w$ according to the dual Plücker matrix ${\mathit{L}}_w^{\ast }$:

$${\mathit{L}}_w^{\ast }=\left[\begin{array}{cc}{\left[{\mathbf{d}}_w\right]}_{\times }& {\mathbf{n}}_w\\ -{\mathbf{n}}_w^{\mathrm{T}}& 0\end{array}\right]={\mathit{\pi }}_i{\mathit{\pi }}_j^{\mathrm{T}}-{\mathit{\pi }}_j{\mathit{\pi }}_i^{\mathrm{T}},$$

(4)

2.2.3. Orthonormal Representation

Since spatial lines only have 4-DoF, there is a problem of overparameterization using the Plücker Coordinates to represent spatial lines. In contrast, the orthonormal representation $(U,W)\in \mathbf{s}O(3)\times \mathbf{s}O(2)$ is more suitable for nonlinear optimization. Let $U=\mathbf{R}(\mathit{\psi })$ and $\mathbf{W}=\mathbf{R}(\varphi )$ denote 3D and 2D rotation matrix, respectively, then we have:

$$U=\mathbf{R}(\mathit{\psi })=\left[{\mathbf{u}}_1,{\mathbf{u}}_2,{\mathbf{u}}_3\right]=\left[\frac{{\mathbf{n}}_w}{‖{\mathbf{n}}_w‖},\frac{{\mathbf{d}}_w}{‖{\mathbf{d}}_w‖},\frac{{\mathbf{n}}_w\times {\mathbf{d}}_w}{‖{\mathbf{n}}_w\times {\mathbf{d}}_w‖}\right],$$

(5)

$$\begin{array}{l}\mathbf{W}=\mathbf{R}(\mathit{\varphi })=\left[\begin{array}{cc}{w}_1& -w_2\\ w_2& w_1\end{array}\right]=\left[\begin{array}{cc}\cos (\mathit{\varphi })& -\sin (\mathit{\varphi })\\ \sin (\mathit{\varphi })& \cos (\mathit{\varphi })\end{array}\right]\\ =\frac{1}{\sqrt{\left({‖{\mathbf{n}}_w‖}^2+{‖{\mathbf{d}}_w‖}^2\right)}}\left[\begin{array}{cc}‖{\mathbf{n}}_w‖& -‖{\mathbf{d}}_w‖\\ ‖{\mathbf{d}}_w‖& ‖{\mathbf{n}}_w‖\end{array}\right],\end{array}$$

(6)

where $\mathit{\psi }={\left[{\psi }_1,{\psi }_2,{\psi }_3\right]}^{\mathrm{T}}$ and $\mathit{\varphi }$ are the 3D rotation angles around the x-, y-, and z-axis of the camera frame and the 2D rotation angle.

Therefore, we can define the orthonormal representation of a spatial line by a four-dimensional vector:

$$o={(\mathit{\psi },\mathit{\varphi })}^{\mathrm{T}},$$

(7)

In addition, given an orthonormal representation $(U,\mathbf{W})$, the corresponding Plücker Coordinates can be obtained by:

$${L^{\prime }}_w={\left[w_1{\mathbf{u}}_1^{\mathrm{T}},w_2{\mathbf{u}}_2^{\mathrm{T}}\right]}^{\mathrm{T}}=\frac{1}{\sqrt{{‖\mathbf{n}‖}^2+{‖\mathbf{d}‖}^2}}{L}_w,$$

(8)

where $w_1=\cos (\mathit{\varphi })$, $w_2=\sin (\mathit{\varphi })$, and ${\mathbf{u}}_i$ is the ith column of $U$. $L_w$ and ${L^{\prime }}_w$ have a scale factor, but they denote the same spatial line.

2.2.4. Line Feature Reprojection Residual

The line feature reprojection residual is defined in terms of point-to-line distance. Given a spatial line $L_c={({\mathbf{n}}_c^{\mathrm{T}},{\mathbf{d}}_c^{\mathrm{T}})}^{\mathrm{T}}$, it can be projected to the image plane by [24]:

$$\mathbf{l}={\left[l_1,l_2,l_3\right]}^{\mathrm{T}}={\mathbf{K}}_L{\mathbf{n}}_c,$$

(9)

where ${\mathbf{K}}_L$ is the line projection matrix.

Finally, assume that $L_w^j$ represents the jth spatial line ${\mathcal{L}}_j$, which is observed by the ith camera frame $c_i$. Then the spatial line reprojection residual can be modeled as:

$$r_L\left({\tilde{\mathit{z}}}_{{\mathcal{L}}_j}^{c_i},\chi \right)=\left[\begin{array}{c}d\left({\mathbf{s}}_{{\mathcal{L}}_j}^{c_i},l_{{\mathcal{L}}_j}^{c_i}\right)\\ d\left({\mathbf{e}}_{{\mathcal{L}}_j}^{c_i},l_{{\mathcal{L}}_j}^{c_i}\right)\end{array}\right]$$

(10)

where ${\mathbf{s}}_{{\mathcal{L}}_j}^{c_i}={[u_{s_j}^{c_i},v_{s_j}^{c_i},1]}^{\mathrm{T}}$ and ${\mathbf{e}}_{{\mathcal{L}}_j}^{c_i}={[u_{e_j}^{c_i},v_{e_j}^{c_i},1]}^{\mathrm{T}}$ denote the two endpoints of the line segment projected on the normalized image plane. $d(\mathbf{s},\mathbf{l})$ is the point-to-line distance:

$$d(\mathbf{s},\mathbf{l})=\frac{{\mathbf{s}}^{\mathrm{T}}\mathbf{l}}{\sqrt{l_1^2+l_2^2}},$$

(11)

For the corresponding Jacobian matrix, we followed the routine of [10].

2.3. GNSS Measurements

GNSS measurements include pseudorange, carrier phase, and Doppler frequency shift.

2.3.1. Pseudorange Measurement

The pseudorange is defined as the measured distance obtained by multiplying the travel time of the satellite signal by the speed of light. Due to the influence of satellite clock error, receiver clock error, and ionospheric and tropospheric delays, the pseudorange is prefixed with “pseudo” to distinguish it from the true distance from the satellite to the GNSS receiver. Generally, although the pseudorange measurement only has meter-level positioning precision (the positioning precision of P code is about 10 m, and the positioning precision of C/A code is 20 m to 30 m), it is real-time and has no ambiguity. Therefore, the pseudorange measurement is still very important for GNSS positioning technology. For a certain satellite $s_j$ and a GNSS receiver $r_k$ at time k, the pseudorange ${\tilde{P}}_{r_k}^{s_j}$ can be modeled as:

$${\tilde{P}}_{r_k}^{s_j}=‖{\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E‖+c\left(\delta {\mathbf{t}}_{r_k}-\delta t_{s_j}\right)+{\mathrm{T}}_{r_k}^{s_j}+I_{r_k}^{s_j}+{\epsilon }_{p_k^j},$$

(12)

where ${\mathbf{p}}_{s_j}^E$ and ${\mathbf{p}}_{r_k}^E$ represent the position of satellite $s_j$ and the GNSS receiver $r_k$ at time k in the ECEF frame, respectively. $c$ is the speed of light. $\delta t_{s_j}$, $T_{r_k}^{s_j}$, and $I_{r_k}^{s_j}$ are the satellite clock error and tropospheric and ionospheric delays, which can be computed according to the satellite ephemeris. ${\epsilon }_{p_k^j}\sim \left(0,{\sigma }_{p_k^j}^2\right)$ denotes the multipath and random error of the pseudorange measurement, which is subject to the Zero-mean Gaussian distribution.

2.3.2. Carrier Phase Measurement

Although pseudorange positioning is an important method for GNSS, its error is too large for some applications that require high-precision positioning. In contrast, due to the short wavelength of the carrier phase (${\lambda }_{L_1}=19\mathrm{cm}$ and ${\lambda }_{L_2}=24\mathrm{cm}$), if the carrier phase measurement is used for positioning, it can achieve centimeter-level positioning accuracy. However, since the carrier phase is a periodic sinusoidal signal, and the GNSS receiver can only measure the part of less than one wavelength, there is the problem of the whole-cycle ambiguity, which makes the positioning process time-consuming. The carrier phase is defined as the phase difference between the phase transmitted by the satellite and the reference phase generated by the GNSS receiver. Similar to the pseudorange measurement, the carrier phase measurement is also related to the position of satellite and GNSS receiver. The carrier phase measurement can be modeled as:

$${\tilde{\Phi }}_{r_k}^{s_j}=‖{\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E‖+c\left(\delta {\mathbf{t}}_{r_k}-\delta t_{s_j}\right)+T_{r_k}^{s_j}-I_{r_k}^{s_j}+\lambda N+{\epsilon }_{{\Phi }_k^j},$$

(13)

where $\lambda$ is the carrier wavelength, and $N$ is the whole-cycle ambiguity. ${\epsilon }_{{\Phi }_k^j}\sim \left(0,{\sigma }_{{\Phi }_k^j}^2\right)$ represents the multipath and random error of the carrier phase measurement, which is subject to the Zero-mean Gaussian distribution.

2.3.3. Doppler Frequency Shift Measurement

The Doppler effect reveals a phenomenon in the spread of waves, that is, the wavelength of the radiation emitted by objects changes accordingly due to the relative motion of the wave source and the observer. As the wave source moves toward the observer, the wavelength becomes shorter and the frequency becomes higher due to the compression of the wave. On the contrary, as the wave source moves away from the observer, the wavelength becomes longer and the frequency becomes lower. Similarly, the Doppler effect can also occur between the satellite and the GNSS receiver. When a satellite orbits Earth in its elliptical orbit, due to the relative motion between the satellite and the GNSS receiver, the frequency of the satellite signal received by the GNSS receiver changes accordingly. This frequency change is called Doppler frequency shift, which can be modeled as:

$$\Delta {\tilde{f}}_{r_k}^{s_j}=-\frac{1}{\lambda }\left[{\mathit{\kappa }}_k^{\mathrm{T}}\left({\mathbf{v}}_{s_j}^E-{\mathbf{v}}_{r_k}^E\right)+c\left(\delta {\dot{\mathbf{t}}}_{r_k}-\delta {\dot{t}}_{s_j}\right)\right]+{\epsilon }_{\Delta f_k^j},$$

(14)

where ${\mathit{\kappa }}_k=\frac{\left({\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E\right)}{‖{\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E‖}$ is the unit vector pointing along the line of sight from the GNSS receiver $r_k$ to the satellite $s_j$. ${\mathbf{v}}_{s_j}^E$ and ${\mathbf{v}}_{r_k}^E$ denote the velocity of the jth satellite and the receiver at time k in the ECEF frame, respectively. $\delta {\dot{t}}_{s_j}$ is the satellite clock drifting rate, which can be calculated according to the satellite ephemeris. ${\epsilon }_{\Delta f_k^j}\sim \left(0,{\sigma }_{\Delta f_k^j}^2\right)$ represents the multipath and random error of the Doppler frequency shift measurement, which is subject to the Zero-mean Gaussian distribution.

2.4. Carrier Phase Smoothed Pseudorange

As mentioned above, the pseudorange measurement proves fast and efficient, but can only achieve meter-level positioning precision. In contrast, the carrier phase measurement can achieve centimeter-level positioning precision but is required to solve the whole-cycle ambiguity, which is complex and time-consuming. Therefore, to combine the merits of the two measurements, we can use carrier phase smoothed pseudorange (short for smoothed pseudorange) to improve the positioning precision. In general, the positioning precision of the smoothed pseudorange measurement is several times higher than that of the pseudorange measurement. For a single-frequency receiver, when the whole-cycle ambiguity and ionospheric delay are nearly constant within a period of time, the pseudorange can be smoothed by using the carrier phase. The Hatch filter is the most widely used for carrier phase smoothed pseudorange, which assumes that the ionospheric delay is nearly constant among GNSS epochs and then averages the multiepoch whole-cycle ambiguity and ionospheric delay to improve the positioning accuracy of the pseudorange measurement. The carrier phase smoothed pseudorange based on the Hatch filter can be modeled as:

$${\overline{\rho }}_{r_k}^{s_j}=\frac{1}{k_m}{\tilde{P}}_{r_k}^{s_j}+\left(1-\frac{1}{k_m}\right)\left[{\overline{\rho }}_{r_{k-1}}^{s_j}+{\tilde{\Phi }}_{r_k}^{s_j}-{\tilde{\Phi }}_{r_{k-1}}^{s_j}\right]+{\epsilon }_{{\rho }_k^j},1\le k_m\le m,$$

(15)

where m is the smoothed interval of the Hatch filter, usually 20 to 200 epochs. ${\epsilon }_{{\rho }_k^j}\sim \left(0,{\sigma }_{{\rho }_k^j}^2\right)$ denotes the multipath and random error of the carrier phase smoothed pseudorange measurement, which is also subject to the Zero-mean Gaussian distribution. According to Equation (15), the variance of the smoothed pseudorange is related to the variance of the pseudorange and carrier phase measurements. Assuming that the variance at different times is independent, then the variance of the smoothed pseudorange can be modeled as:

$${\sigma }_{{\rho }_k^j}^2=\frac{1}{k_m}{\sigma }_{P_k^j}^2+\left(1-\frac{1}{k_m}\right)\left({\sigma }_{{\Phi }_k^j}^2+{\sigma }_{{\Phi }_{k-1}^j}^2\right),$$

(16)

2.5. Factor Graph Representation

The factor graph of our system is shown in Figure 3, which shows the factors in a sliding window, including point feature factors, line feature factors, IMU factors, carrier phase smoothed pseudorange factors, and Doppler frequency shift factors. Visual observation consists of the point and line features detected by our system. In nonlinear optimization, the states are optimized according to the residuals of these factors. The states of our system include:

$$\begin{array}{l}\mathcal{X}=\left[x_0,x_1,\cdots x_n,{\mu }_0,{\mu }_1,\cdots {\mu }_i,o_0,o_1,\cdots ,o_j,{\mathbf{p}}_c^b,{\mathbf{q}}_c^b,\mathit{\phi },{\mathbf{p}}_N^E,{\mathbf{p}}_r^b\right],\\ x_k=\left[{\mathbf{p}}_{b_k}^w,{\mathbf{v}}_{b_k}^w,{\mathbf{q}}_{b_k}^w,{\mathbf{b}}_a,{\mathbf{b}}_g,\delta {\mathbf{t}}_{r_k},\delta {\dot{\mathbf{t}}}_{r_k}\right],k=0,1,\cdots ,n,\\ o=\left[\mathit{\psi },\mathit{\varphi }\right],\end{array}$$

(17)

where n is the sliding window size, i is the number of point features, and j is the number of line features. ${\mu }_i$ is the inverse depth of the ith point feature in the sliding window.

The IMU preintegration and point feature residuals can be obtained according to [16], and the line feature residual can be obtained from Equation (10). In the following, we compute the smoothed pseudorange and Doppler frequency shift residuals in detail. The position of the receiver at time k in the ECEF frame is:

$${\mathbf{p}}_{r_k}^E={\mathbf{R}}_N^E{\mathbf{R}}_w^N{\mathbf{p}}_{r_k}^w+{\mathbf{p}}_N^E,$$

(18)

where ${\mathbf{p}}_{r_k}^w$ is the position of the receiver in the local world frame, and it can be modeled as:

$${\mathbf{p}}_{r_k}^w={\mathbf{R}}_{b_k}^w{\mathbf{p}}_r^b+{\mathbf{p}}_{b_k}^w,$$

(19)

${\mathbf{R}}_w^N$ is the rotation matrix of the local world frame with respect to the ENU frame. Since the two frames are gravity-aligned, the 1-DoF ${\mathbf{R}}_w^N$ is only related to the yaw offset $\mathit{\varphi }$ and can be modeled as:

$${\mathbf{R}}_w^N=\left[\begin{array}{ccc}cos\mathit{\varphi }& -si\mathbf{n}\mathit{\varphi }& 0\\ si\mathbf{n}\mathit{\varphi }& cos\mathit{\varphi }& 0\\ 0& 0& 1\end{array}\right],$$

(20)

${\mathbf{R}}_N^E$ represents the rotation matrix of the ENU frame with respect to the ECEF frame, which is determined by the longitude L and latitude B of ${\mathbf{p}}_N^E$. Given a position ${\mathbf{p}}_N^E={[X,Y,Z]}^{\mathrm{T}}$ in the ECEF frame, it can be denoted in the geodetic frame as:

$$\begin{array}{l}\left\{\begin{array}{l}L=\arctan \left(Y/X\right)\\ B=\arctan \left\{Z\left(N_r+H\right)/\left[\sqrt{X^2+Y^2}\left(N_r\left(1-e^2\right)+H\right)\right]\right\}\\ H=Z/\sin B-N_r\left(1-e^2\right)\end{array}\right.,\\ N_r=a/\sqrt{1-e^2{\sin }^{2}B},e^2=\left(a^2-b^2\right)/a^2,\end{array}$$

(21)

where a and b denote the semimajor axis and the semiminor axis of the elliptical orbit, respectively. $N_r$ is the radius of curvature in prime vertical, and e is the eccentricity that is related to a and b.

After the longitude L and latitude B of ${\mathbf{p}}_N^E$ are computed, ${\mathbf{R}}_N^E$ can be obtained by:

$${\mathbf{R}}_N^E=\left[\begin{array}{ccc}-\sin L& -\sin B\cos L& \cos B\cos L\\ \cos L& -\sin B\sin L& \cos B\sin L\\ 0& \cos B& \sin B\end{array}\right],$$

(22)

According to Equation (15), the smoothed pseudorange residual can be represented as:

$$r_{\rho }({\tilde{z}}_{r_k}^{s_j},\mathcal{X})=\frac{1}{k_m}{\tilde{P}}_{r_k}^{s_j}+\left(1-\frac{1}{k_m}\right)\left[{\overline{\rho }}_{r_{k-1}}^{s_j}+{\tilde{\Phi }}_{r_k}^{s_j}-{\tilde{\Phi }}_{r_{k-1}}^{s_j}\right]-{\overline{\rho }}_{r_k}^{s_j},$$

(23)

In nonlinear optimization, it is necessary to calculate the Jacobian matrix of the smoothed pseudorange residual with respect to the states. Therefore, from Equation (23), the Jacobian matrix of the smoothed pseudorange can be obtained as:

$${\mathbf{J}}_{\rho }=\left[\begin{array}{ccccccccc}\frac{\partial r_{\rho }}{\partial {\mathbf{p}}_{b_{k-1}}^w}& \frac{\partial r_{\rho }}{\partial {\mathbf{R}}_{b_{k-1}}^w}& \frac{\partial r_{\rho }}{\partial {\mathbf{p}}_{b_k}^w}& \frac{\partial r_{\rho }}{\partial {\mathbf{R}}_{b_k}^w}& \frac{\partial r_{\rho }}{\partial \delta {\mathbf{t}}_{r_{k-1}}}& \frac{\partial r_{\rho }}{\partial \delta {\mathbf{t}}_{r_k}}& \frac{\partial r_{\rho }}{\partial \mathit{\phi }}& \frac{\partial r_{\rho }}{\partial {\mathbf{p}}_N^E}& \frac{\partial r_{\rho }}{\partial {\mathbf{p}}_r^b}\end{array}\right],$$

(24)

with

$$\begin{array}{l}\frac{\partial r_{\rho }}{\partial {\mathbf{p}}_{b_{k-1}}^w}=\left(1-\frac{1}{k_m}\right)\frac{{\left({\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_{k-1}}^E\right)}^{\mathrm{T}}}{‖{\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_{k-1}}^E‖}{\mathbf{R}}_N^E{\mathbf{R}}_w^N,\\ \frac{\partial r_{\rho }}{\partial {\mathbf{R}}_{b_{k-1}}^w}=-\left(1-\frac{1}{k_m}\right)\frac{{\left({\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_{k-1}}^E\right)}^{\mathrm{T}}}{‖{\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_{k-1}}^E‖}{\mathbf{R}}_N^E{\mathbf{R}}_w^N{\mathbf{R}}_{b_{k-1}}^w{\left[{\mathbf{p}}_r^b\right]}_{\times },\\ \frac{\partial r_{\rho }}{\partial {\mathbf{p}}_{b_k}^w}=-\frac{{\left({\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E\right)}^{\mathrm{T}}}{‖{\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E‖}{\mathbf{R}}_N^E{\mathbf{R}}_w^N,\\ \frac{\partial r_{\rho }}{\partial {\mathbf{R}}_{b_k}^w}=\frac{{\left({\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E\right)}^{\mathrm{T}}}{‖{\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E‖}{\mathbf{R}}_N^E{\mathbf{R}}_w^N{\mathbf{R}}_{b_k}^w{\left[{\mathbf{p}}_r^b\right]}_{\times },\\ \frac{\partial r_{\rho }}{\partial \delta {\mathbf{t}}_{r_{k-1}}}=-c\left(1-\frac{1}{k_m}\right),\\ \frac{\partial r_{\rho }}{\partial \delta {\mathbf{t}}_{r_k}}=c,\\ \frac{\partial r_{\rho }}{\partial \mathit{\phi }}=-\frac{{\left({\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E\right)}^{\mathrm{T}}}{‖{\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E‖}{\mathbf{R}}_N^E{\dot{\mathbf{R}}}_w^N{\mathbf{p}}_{r_k}^w+\left(1-\frac{1}{k_m}\right)\frac{{\left({\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_{k-1}}^E\right)}^{\mathrm{T}}}{‖{\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_{k-1}}^E‖}{\mathbf{R}}_N^E{\dot{\mathbf{R}}}_w^N{\mathbf{p}}_{r_{k-1}}^w,\\ \frac{\partial r_{\rho }}{\partial {\mathbf{p}}_N^E}=-\frac{{\left({\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E\right)}^{\mathrm{T}}}{‖{\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E‖}+\left(1-\frac{1}{k_m}\right)\frac{{\left({\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_{k-1}}^E\right)}^{\mathrm{T}}}{‖{\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_{k-1}}^E‖},\\ \frac{\partial r_{\rho }}{\partial {\mathbf{p}}_r^b}=-\frac{{\left({\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E\right)}^{\mathrm{T}}}{‖{\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E‖}{\mathbf{R}}_N^E{\mathbf{R}}_w^N{\mathbf{R}}_{b_k}^w+\left(1-\frac{1}{k_m}\right)\frac{{\left({\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_{k-1}}^E\right)}^{\mathrm{T}}}{‖{\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_{k-1}}^E‖}{\mathbf{R}}_N^E{\mathbf{R}}_w^N{\mathbf{R}}_{b_{k-1}}^w,\end{array}$$

(25)

where

$${\dot{\mathbf{R}}}_w^N=\frac{d{\mathbf{R}}_w^N}{d\mathit{\phi }}=\left[\begin{array}{ccc}-\mathrm{s}\mathrm{i}\mathrm{n}\mathit{\varphi }& -\mathrm{c}\mathrm{o}\mathrm{s}\mathit{\varphi }& 0\\ \mathrm{c}\mathrm{o}\mathrm{s}\mathit{\varphi }& -\mathrm{s}\mathrm{i}\mathrm{n}\mathit{\varphi }& 0\\ 0& 0& 0\end{array}\right],$$

(26)

The derivation details are provided in Appendix A.

In Equation (14), the velocity of the receiver in the ECEF frame is transformed from that in the local world frame:

$${\mathbf{v}}_{r_k}^E\simeq {\mathbf{R}}_N^E{\mathbf{R}}_w^N{\mathbf{v}}_{b_k}^w,$$

(27)

Therefore, the Doppler frequency shift residual can be obtained by:

$$r_{\Delta f}({\tilde{z}}_{r_k}^{s_j},\mathcal{X})=\frac{1}{\lambda }\left[{\mathit{\kappa }}_k^{\mathrm{T}}\left({\mathbf{v}}_{s_j}^E-{\mathbf{v}}_{r_k}^E\right)+c\left(\delta {\dot{\mathbf{t}}}_{r_k}-\delta {\dot{t}}_{s_j}\right)\right]+\Delta {\tilde{f}}_{r_k}^{s_j},$$

(28)

Similar to the smoothed pseudorange, the Jacobian matrix of the Doppler frequency shift is:

$${\mathbf{J}}_{\Delta f}=\left[\begin{array}{ccccccc}\frac{\partial r_{\Delta f}}{\partial {\mathbf{p}}_{b_k}^w}& \frac{\partial r_{\Delta f}}{\partial {\mathbf{R}}_{b_k}^w}& \frac{\partial r_{\Delta f}}{\partial {\mathbf{v}}_{b_k}^w}& \frac{\partial r_{\Delta f}}{\partial \delta {\dot{\mathbf{t}}}_{r_k}}& \frac{\partial r_{\Delta f}}{\partial \mathit{\phi }}& \frac{\partial r_{\Delta f}}{\partial {\mathbf{p}}_N^E}& \frac{\partial r_{\Delta f}}{\partial {\mathbf{p}}_r^b}\end{array}\right]$$

(29)

with

$$\begin{array}{l}\frac{\partial r_{\Delta f}}{\partial {\mathbf{p}}_{b_k}^w}=-{\left({\mathbf{v}}_{s_j}^E-{\mathbf{v}}_{r_k}^E\right)}^{\mathrm{T}}\frac{\mathbf{I}{‖{\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E‖}^2-\left({\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E\right){\left({\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E\right)}^{\mathrm{T}}}{\lambda {‖{\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E‖}^3}{\mathbf{R}}_N^E{\mathbf{R}}_w^N,\\ \frac{\partial r_{\Delta f}}{\partial {\mathbf{R}}_{b_k}^w}={\left({\mathbf{v}}_{s_j}^E-{\mathbf{v}}_{r_k}^E\right)}^{\mathrm{T}}\frac{\mathbf{I}{‖{\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E‖}^2-\left({\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E\right){\left({\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E\right)}^{\mathrm{T}}}{\lambda {‖{\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E‖}^3}{\mathbf{R}}_N^E{\mathbf{R}}_w^N{\mathbf{R}}_{{\mathbf{b}}_k}^w{\left[{\mathbf{p}}_r^{\mathbf{b}}\right]}_{\times },\\ \frac{\partial r_{\Delta f}}{\partial {\mathbf{v}}_{{\mathbf{b}}_k}^w}=-\frac{1}{\lambda }{\mathit{\kappa }}_k^{\mathrm{T}}{\mathbf{R}}_N^E{\mathbf{R}}_w^N,\\ \frac{\partial r_{\Delta f}}{\partial \delta {\dot{\mathbf{t}}}_{r_k}}=\frac{c}{\lambda },\\ \frac{\partial r_{\Delta f}}{\partial \mathit{\varphi }}\approx -\frac{1}{\lambda }{\mathit{\kappa }}_k^{\mathrm{T}}{\mathbf{R}}_N^E{\dot{\mathbf{R}}}_w^N{\mathbf{v}}_{b_k}^w,\\ \frac{\partial r_{\Delta f}}{\partial {\mathbf{p}}_N^E}\approx 0,\\ \frac{\partial r_{\Delta f}}{\partial {\mathbf{p}}_r^{\mathbf{b}}}=-{\left({\mathbf{v}}_{s_j}^E-{\mathbf{v}}_{r_k}^E\right)}^{\mathrm{T}}\frac{\mathbf{I}{‖{\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E‖}^2-\left({\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E\right){\left({\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E\right)}^{\mathrm{T}}}{\lambda {‖{\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_k}^E‖}^3}{\mathbf{R}}_N^E{\mathbf{R}}_w^N{\mathbf{R}}_{{\mathbf{b}}_k}^w,\end{array}$$

(30)

The corresponding derivation rule is similar to the Jacobian matrix of the smoothed pseudorange and will not be repeated here.

After the residuals of all factors are obtained, then the system can optimize the states by Ceres solver [25]. The cost function of our system is:

$$\begin{array}{c}\underset{\mathcal{X}}{\min }\left\{{‖r_p-{\mathbf{H}}_p\mathcal{X}‖}^2+{\displaystyle \sum _{k\in \mathcal{B}}{‖r_{\mathcal{B}}({\tilde{z}}_{b_{k+1}}^{b_k},\mathcal{X})‖}_{{\mathbf{P}}_{b_{k+1}}^{b_k}}^2}+{\displaystyle \sum _{(l,j)\in \mathcal{C}}\alpha \left({‖r_{\mathcal{C}}({\tilde{z}}_l^{c_j},\mathcal{X})‖}_{{\mathbf{P}}_l^{c_j}}^2\right)}\right.\\ +{\displaystyle \sum }_{\left(i,j\right)\in L}\alpha \left(r_L{\left(z{˜}_{{}_j}^{c_i},\right)}_{{\mathbf{P}}_{{}_j}^{c_i}}^2\right)+{\displaystyle \sum _{(k,j)\in \rho }{‖r_{\rho }({\tilde{z}}_{r_k}^{s_j},\mathcal{X})‖}_{{\sigma }_{\rho }^2}^2}\left.+{\displaystyle \sum _{(k,j)\in \Delta f}{‖r_{\Delta f}({\tilde{z}}_{r_k}^{s_j},\mathcal{X})‖}_{{\sigma }_{\Delta f}^2}^2}\right\},\end{array}$$

(31)

where $r_p$ is the prior residual for marginalization. $\mathcal{B}$ is the set of IMU preintegration measurements in the sliding window. $\mathcal{C}$ and $L$ are the set of point and line features in the sliding window, respectively. $\alpha$ is the Cauchy robust function used to suppress outliers.

2.6. GNSS-IMU Calibration

${\mathbf{p}}_r^b$ is the extrinsic translation parameter between the GNSS receiver and IMU. After our system performs ENU Origin Estimation and Yaw Estimation, then we can calibrate ${\mathbf{p}}_r^b$. We estimate ${\mathbf{p}}_r^b$ as the initial value of nonlinear optimization through the following optimization problem:

$$\underset{{\mathbf{p}}_r^b}{\min }{\displaystyle \sum _{(k,j)\in \Delta f}{‖r_{\Delta f}({\tilde{z}}_{r_k}^{s_j},\mathcal{X})‖}_{{\sigma }_{\Delta f}^2}^2},$$

(32)

After the GNSS-IMU calibration is successfully initialized, our system performs the nonlinear optimization.

2.7. Observability Analysis of Tightly Coupled GNSS-VIO System

A SLAM system can be described using a state equation and an output equation, where the input and output are the external variables of the system, and the state is the internal variables of the system. If the states of the system can be completely represented by the output, the system is fully observable; otherwise, the system is not fully observable. Observability plays a very important role in the state estimation problem. If some states of the system are not observable, the positioning precision of the system will be affected when running in long trajectories. The observability of the system can be represented by the observability matrix. If the dimension of the null space of the observability matrix is equal to 0, then the system is fully observable. To facilitate the observability analysis of our proposed system, some simplifications were required. First, the accelerometer and gyroscope biases were not included in the states, because the biases were observable and they did not change the results of the observability analysis [26]. Second, we considered a single point and line feature [27]. Third, the translation parameter ${\mathbf{p}}_r^b$ was successfully calibrated. Then the discrete-time linear error state model and residual of the system are:

$$\begin{array}{l}\delta {\mathbf{x}}_{k+1}\simeq {\mathbf{\Phi }}_k\delta {\mathbf{x}}_k+{\mathbf{w}}_k,\\ r_k={\mathbf{H}}_k\delta {\mathbf{x}}_k+{\mathbf{n}}_k,\end{array}$$

(33)

where $\delta {\mathbf{x}}_k$ is the error state, and $r_k$ is the residual. ${\mathbf{\Phi }}_k$ and ${\mathbf{H}}_k$ are the error-state transition matrix and the measurement Jacobian matrix, respectively. ${\mathbf{w}}_k$ and ${\mathbf{n}}_k$ represent the system noise process and the measurement noise process, respectively. The noise process is modeled as a Zero-mean white Gaussian process.

According to [28], the observability matrix can be obtained as:

$$\mathbf{M}=\left[\begin{array}{c}{\mathbf{H}}_k\\ {\mathbf{H}}_{k+1}{\mathbf{\Phi }}_k\\ ⋮\\ {\mathbf{H}}_{k+t}{\mathbf{\Phi }}_{k+t-1}\cdots {\mathbf{\Phi }}_k\end{array}\right],$$

(34)

In Equation (34), the observability matrix is defined as a function of the error-state transition matrix ${\mathbf{\Phi }}_k$ and the measurement Jacobian matrix ${\mathbf{H}}_k$.

Therefore, given the linearized system in Equation (33), its observability can be demonstrated according to Equation (34). The proof is as follows:

Theorem 1.

The states represented in the ECEF frame are fully observable.

Proof of Theorem1.

The simplified states include:

$${\mathbf{x}}_k^E=\left[{\mathbf{p}}_{b_k}^E,{\mathbf{v}}_{b_k}^E,{\mathbf{q}}_{b_k}^E,\mu ,o,\mathit{\varphi },{\mathbf{p}}_N^E\right],$$

(35)

Generally, the raw accelerometer and gyroscope measurements from IMU can be obtained by:

$$\begin{array}{l}{\tilde{\mathit{a}}}_b={\mathbf{R}}_w^b({\mathit{a}}_w+{\mathit{g}}_w)+{\mathbf{b}}_a+{\mathit{n}}_a,\\ {\tilde{\mathit{\omega }}}_b={\mathit{\omega }}_b+{\mathbf{b}}_g+{\mathit{n}}_g,\end{array}$$

(36)

where ${\tilde{\mathit{a}}}_b$ and ${\tilde{\mathit{\omega }}}_b$ are the accelerometer and gyroscope measurements. The measurements are represented in the local world frame.

Given two time instants, position, velocity, and orientation states can be propagated by the IMU measurements:

$$\begin{array}{l}{\mathbf{p}}_{b_{k+1}}^w={\mathbf{p}}_{b_k}^w+{\mathbf{v}}_{b_k}^w\Delta t_k+{\displaystyle \mathrm{\int }{\displaystyle {\mathrm{\int }}_{t\in (t_k,t_{k+1})}\left[{\mathbf{R}}_{b_t}^w\left({\tilde{\mathit{a}}}_{b_t}-{\mathbf{b}}_a\right)-{\mathit{g}}_w\right]d{t}^2}},\\ {\mathbf{v}}_{b_{k+1}}^w={\mathbf{v}}_{b_k}^w+{\displaystyle {\mathrm{\int }}_{t\in (t_k,t_{k+1})}\left[{\mathbf{R}}_{b_t}^w\left({\tilde{\mathit{a}}}_{b_t}-{\mathbf{b}}_a\right)-{\mathit{g}}_w\right]dt,}\\ {\mathbf{q}}_{b_{k+1}}^w={\displaystyle {\mathrm{\int }}_{t\in (t_k,t_{k+1})}{\mathbf{q}}_{b_t}^w\otimes \left[\begin{array}{c}0\\ \frac{1}{2}\left({\tilde{\mathit{\omega }}}_{b_t}-{\mathbf{b}}_g\right)\end{array}\right]dt},\end{array}$$

(37)

where $\otimes$ denotes the quaternion multiplication operation.

Equation (37) is the continuous state propagation model. To analyze the observability of the system, it was necessary to perform the discretization of the continuous-time system model. Therefore, we used the Euler method to compute the discrete-time model of Equation (37):

$$\begin{array}{l}{\mathbf{p}}_{b_{k+1}}^w={\mathbf{p}}_{b_k}^w+{\mathbf{v}}_{b_k}^w\Delta t_k+\frac{1}{2}\left[{\mathbf{R}}_{b_k}^w\left({\tilde{\mathit{a}}}_{b_k}-{\mathbf{b}}_a\right)-{\mathit{g}}_w\right]\Delta t_k^2,\\ {\mathbf{v}}_{b_{k+1}}^w={\mathbf{v}}_{b_k}^w+\left[{\mathbf{R}}_{b_k}^w\left({\tilde{\mathit{a}}}_{b_k}-{\mathbf{b}}_a\right)-{\mathit{g}}_w\right]\Delta t_k,\\ {\mathbf{q}}_{b_{k+1}}^w={\mathbf{q}}_{b_k}^w\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\left({\tilde{\mathit{\omega }}}_{b_k}-{\mathbf{b}}_g\right)\Delta t_k\end{array}\right],\end{array}$$

(38)

According to Equation (38), the error propagation equation of position, velocity, and orientation can be obtained by:

$$\left[\begin{array}{l}\delta {\mathbf{p}}_{b_{k+1}}^w\\ \delta {\mathbf{v}}_{b_{k+1}}^w\\ \delta {\theta }_{b_{k+1}}^w\end{array}\right]=\left[\begin{array}{lll}\mathbf{I}& \Delta t_k\mathbf{I}& -\frac{1}{2}{\mathbf{R}}_{b_k}^w{\left[{\tilde{\mathit{a}}}_{b_k}-{\mathbf{b}}_a\right]}_{\times }\Delta t_k^2\\ \mathbf{0}& \mathbf{I}& -{\mathbf{R}}_{b_k}^w{\left[{\tilde{\mathit{a}}}_{b_k}-{\mathbf{b}}_a\right]}_{\times }\Delta t_k\\ \mathbf{0}& \mathbf{0}& \mathbf{I}-{\left[{\tilde{\mathit{\omega }}}_{b_k}-{\mathbf{b}}_g\right]}_{\times }\Delta t_k\end{array}\right]\left[\begin{array}{c}\delta {\mathbf{p}}_{b_k}^w\\ \delta {\mathbf{v}}_{b_k}^w\\ \delta {\mathit{\theta }}_{b_k}^w\end{array}\right]+{{\mathbf{w}}^{\prime }}_k,$$

(39)

The details of Equation (39) are provided in Appendix B.

Similarly, when the states are represented in the ECEF frame, Equation (39) is still held, namely:

$$\left[\begin{array}{l}\delta {\mathbf{p}}_{b_{k+1}}^E\\ \delta {\mathbf{v}}_{b_{k+1}}^E\\ \delta {\theta }_{b_{k+1}}^E\end{array}\right]=\left[\begin{array}{lll}\mathbf{I}& \Delta t_k\mathbf{I}& -\frac{1}{2}{\mathbf{R}}_{b_k}^E{\left[{\tilde{\mathit{a}}}_{b_k}-{\mathbf{b}}_a\right]}_{\times }\Delta t_k^2\\ \mathbf{0}& \mathbf{I}& -{\mathbf{R}}_{b_k}^E{\left[{\tilde{\mathit{a}}}_{b_k}-{\mathbf{b}}_a\right]}_{\times }\Delta t_k\\ \mathbf{0}& \mathbf{0}& \mathbf{I}-{\left[{\tilde{\mathit{\omega }}}_{b_k}-{\mathbf{b}}_g\right]}_{\times }\Delta t_k\end{array}\right]\left[\begin{array}{c}\delta {\mathbf{p}}_{b_k}^E\\ \delta {\mathbf{v}}_{b_k}^E\\ \delta {\mathit{\theta }}_{b_k}^E\end{array}\right]+{{\mathbf{w}}^{\prime }}_k,$$

(40)

Therefore, the discrete-time error state model of Equation (35) can be obtained:

$$\begin{array}{l}\left[\begin{array}{c}\delta {\mathbf{p}}_{b_{k+1}}^E\\ \delta {\mathbf{v}}_{b_{k+1}}^E\\ \delta {\mathit{\theta }}_{b_{k+1}}^E\\ \delta {\mu }_{k+1}\\ \delta o_{k+1}\\ \delta {\phi }_{k+1}\\ \delta {\mathbf{p}}_N^E{}_{(k+1)}\end{array}\right]=\left[\begin{array}{cccc}{\mathbf{I}}_3& \Delta t_k{\mathbf{I}}_3& {\mathbf{\Phi }}_k^1& {\mathbf{0}}_{3\times 9}\\ {\mathbf{0}}_3& {\mathbf{I}}_3& {\mathbf{\Phi }}_k^2& {\mathbf{0}}_{3\times 9}\\ {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{\Phi }}_k^3& {\mathbf{0}}_{3\times 9}\\ {\mathbf{0}}_{9\times 3}& {\mathbf{0}}_{9\times 3}& {\mathbf{0}}_{9\times 3}& {\mathbf{I}}_9\end{array}\right]\left[\begin{array}{c}\delta {\mathbf{p}}_{b_k}^E\\ \delta {\mathbf{v}}_{b_k}^E\\ \delta {\mathit{\theta }}_{b_k}^E\\ \delta {\mu }_k\\ \delta o_k\\ \delta {\phi }_k\\ \delta {\mathbf{p}}_N^E{}_{(k)}\end{array}\right]+{\mathbf{w}}_k,\\ \Rightarrow \delta {\mathbf{x}}_{k+1}^E={\mathbf{\Phi }}_k\delta {\mathbf{x}}_k^E+{\mathbf{w}}_k,\end{array}$$

(41)

where

$$\left\{\begin{array}{l}{\mathbf{\Phi }}_k^1=-\frac{1}{2}{\mathbf{R}}_{b_k}^E{\left[{\tilde{\mathit{a}}}_{b_k}-{\mathbf{b}}_a\right]}_{\times }\Delta t_k^2\\ {\mathbf{\Phi }}_k^2=-{\mathbf{R}}_{b_k}^E{\left[{\tilde{\mathit{a}}}_{b_k}-{\mathbf{b}}_a\right]}_{\times }\Delta t_k\\ {\mathbf{\Phi }}_k^3=\mathbf{I}-{\left[{\tilde{\mathit{\omega }}}_{b_k}-{\mathbf{b}}_g\right]}_{\times }\Delta t_k\end{array}\right.,$$

(42)

Since ${\mathbf{\Phi }}_k$ is an upper triangular matrix, the error-state transition matrix ${\mathbf{\Phi }}_{k+t,k}$ from time step k to k + t is also an upper triangular matrix, namely:

$$\begin{array}{l}{\mathbf{\Phi }}_{k+t,k}={\mathbf{\Phi }}_{k+t-1}\cdots {\mathbf{\Phi }}_{k+1}{\mathbf{\Phi }}_k\\ =\left[\begin{array}{llll}{\mathbf{I}}_3& (t_{k+t-1}-t_{k-1}){\mathbf{I}}_3& {\mathbf{\Phi }}_{13}^{k+t}& {\mathbf{0}}_{3\times 9}\\ {\mathbf{0}}_3& {\mathbf{I}}_3& {\mathbf{\Phi }}_{23}^{k+t}& {\mathbf{0}}_{3\times 9}\\ {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{\Phi }}_{33}^{k+t}& {\mathbf{0}}_{3\times 9}\\ {\mathbf{0}}_{9\times 3}& {\mathbf{0}}_{9\times 3}& {\mathbf{0}}_{9\times 3}& {\mathbf{I}}_9\end{array}\right],\end{array}$$

(43)

where ${\mathbf{\Phi }}_{13}^{k+t}$, ${\mathbf{\Phi }}_{23}^{k+t}$, and ${\mathbf{\Phi }}_{33}^{k+t}$ are nonzero entries.

For the measurement Jacobian matrix, our system consists of visual observations and GNSS measurements. GNSS measurements include pseudorange, carrier phase, and Doppler frequency shift, and any of them yields the same results for observability analysis. Therefore, in the following, we used the smoothed pseudorange measurement for observability analysis.

The Jacobian matrix of the smoothed pseudorange measurement with respect to the states in Equation (35) can be obtained by:

$$\begin{array}{ll}{\mathbf{H}}_{k+t}^g& =\left[\begin{array}{llll}{\mathit{p}}_{k+t}& {\mathbf{0}}_{1\times 11}& {\mathit{p}}_{k+t}{\mathbf{R}}_N^E{\dot{\mathbf{R}}}_w^N{\mathbf{p}}_{r_{k+t}}^w-\left(1-\frac{1}{k_m}\right){\mathit{p}}_{k+t-1}{\mathbf{R}}_N^E{\dot{\mathbf{R}}}_w^N{\mathbf{p}}_{r_{k+t-1}}^w& {\mathit{p}}_{k+t}-\left(1-\frac{1}{k_m}\right){\mathit{p}}_{k+t-1}\end{array}\right]\\ & =\left[\begin{array}{cccc}{\mathbf{h}}_{k+t}^1& {\mathbf{0}}_{1\times 11}& {\mathbf{h}}_{k+t}^2& {\mathbf{h}}_{k+t}^3\end{array}\right],\\ {\mathit{p}}_{k+t}& =-\frac{{\left({\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_{k+t}}^E\right)}^{\mathrm{T}}}{‖{\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_{k+t}}^E‖},{\mathit{p}}_{k+t-1}=-\frac{{\left({\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_{k+t-1}}^E\right)}^{\mathrm{T}}}{‖{\mathbf{p}}_{s_j}^E-{\mathbf{p}}_{r_{k+t-1}}^E‖},\end{array}$$

(44)

The derivation details are similar to the Jacobian matrix of the smoothed pseudorange, which has been computed in Appendix A. Since the smoothed pseudorange measurement does not include velocity, orientation, the inverse depth of point feature, and the orthonormal representation of line feature, the corresponding entry of the Jacobian matrix is equal to zero.

In addition, we can obtain the Jacobian matrix of the visual observations by:

$${\mathbf{H}}_{k+t}^v=\left[\begin{array}{cccc}{\mathbf{0}}_{2\times 9}& \frac{\partial r_{\mathcal{C}}}{\partial \delta {\mu }_{k+t}}& {\mathbf{0}}_{2\times 4}& {\mathbf{0}}_{2\times 4}\\ {\mathbf{0}}_{2\times 9}& {\mathbf{0}}_{2\times 1}& \frac{\partial r_L}{\partial \delta o_{k+t}}& {\mathbf{0}}_{2\times 4}\end{array}\right]$$

(45)

where $\frac{\partial r_{\mathcal{C}}}{\partial \delta {\mu }_{k+t}}$ and $\frac{\partial r_L}{\partial \delta o_{k+t}}$ can be obtained from [10].

Therefore, we can obtain the entry of the observability matrix:

$$\left[\begin{array}{l}{\mathbf{H}}_{k+t}^v\\ {\mathbf{H}}_{k+t}^g\end{array}\right]{\mathbf{\Phi }}_{k+t,k}=\left[\begin{array}{llllll}{\mathbf{0}}_{4\times 3}& {\mathbf{0}}_{4\times 3}& {\mathbf{0}}_{4\times 3}& {\mathrm{\Pi }}^{\prime }& {\mathbf{0}}_{4\times 1}& {\mathbf{0}}_{4\times 3}\\ {\mathbf{h}}_{k+t}^1& {\mathbf{h}}_{k+t}^1(t_{k+t-1}-t_{k-1})& {\mathbf{h}}_{k+t}^1{\mathbf{\Phi }}_{13}^{k+t}& {\mathbf{0}}_{3\times 5}& {\mathbf{h}}_{k+t}^2& {\mathbf{h}}_{k+t}^3\end{array}\right]$$

(46)

where

$${\mathrm{\Pi }}^{\prime }=\left[\begin{array}{cc}\frac{\partial r_{\mathcal{C}}}{\partial \delta {\mu }_{k+t}}& {\mathbf{0}}_{2\times 4}\\ {\mathbf{0}}_{2\times 1}& \frac{\partial r_L}{\partial \delta o_{k+t}}\end{array}\right]$$

(47)

According to Equation (46), we can clearly observe that the dimension of the null space of $\mathbf{M}$ is equal to zero, which means the states represented in the ECEF frame are fully observable and the tightly coupled GNSS-VIO state estimator is consistent. □

The fact that the tightly coupled GNSS-VIO system is fully observable means that even if the system runs in long trajectories, the accumulated error can be eliminated. By leveraging the global measurements from GNSS, our system can achieve high-precision and drift-free positioning compared with the VIO system, which has four unobservable directions.

3. System Overview

The architecture of our proposed system is shown in Figure 4.

The proposed system implements four threads including data input, preprocessing, initialization, and nonlinear optimization. As shown in Figure 4, the white block diagrams represent the work that has been implemented by VINS-Mono [16] and GVINS [21], and the green block diagrams represent improvements we made. The inputs of our system are image, IMU, and GNSS measurements. In the preprocessing step, point and line feature detection and tracking were performed, IMU measurements were preintegrated, and the pseudorange measurements were smoothed by the carrier phase. In the initialization step, we followed the routine of VINS-Mono [16] for VI-Alignment. After VI-Alignment was completed, we performed GNSS initialization, which was divided into four stages: ENU Origin Estimation (Coarse), Yaw Estimation, ENU Origin Estimation (Fine), and GNSS-IMU Calibration. The first three stages were implemented in GVINS [21]. Finally, nonlinear optimization was performed. Nonlinear optimization will optimize the states in Equation (17) by leveraging the residuals and Jacobian matrices of different factors, which were computed in Section 2.5.

4. Experimental Results

Our experiments were conducted on the public dataset GVINS-Dataset [29], which captured scenes from the Hong Kong University of Science and Technology. The measurements were collected by a helmet that is equipped with a VI-Sensor and a u-blox ZED-F9P GNSS receiver. The dataset sports field captured a sports field scene where the device followed an athletic track for five laps. The sports field is an outdoor environment with an open area where the satellites are well locked and the RTK solution remains fixed. The other dataset complex environment was a complex indoor–outdoor environment where many challenging scenes were captured. For example, point and line features cannot be detected in bright or dim scenes, and the GNSS signal was highly corrupted or unavailable in cluttered or indoor environments (about 25 m). The overall distance of the complex environment dataset was over 3 km. We compared our proposed system with some open-source SLAM systems, including GVINS and VINS-Mono where VINS-Mono includes results with and without loop closure. For comparison with our results, we transformed the trajectories of VINS-Mono from the local world frame to the ECEF frame. Furthermore, since RTK has centimeter-level positioning precision, we compared it with our trajectories as the ground truth.

4.1. Sports Field

We plotted the trajectories of the sports field on Google Earth as shown in Figure 5. We see that VINS-Mono without loop closure suffers from accumulated drift among all three directions, which leads to the worst positioning precision among methods. Obviously, the drift increases with each lap around the sports field. VINS-Mono with loop closure can significantly improve the positioning precision, but there is still an obvious drift in yaw direction, which is mainly because the VINS-Mono has four unobservable directions. As a comparison, since GVINS is fully observable in the ECEF frame, it has a smaller positioning error and is drift-free. As depicted in Figure 5, its trajectory is very close to RTK. In addition, since our method is also a tightly coupled GNSS-VIO system, which is fully observable in the ECEF frame and the ENU frame, the positioning result of our method is also very accurate.

To quantitatively analyze results, we compared the positioning errors of GVINS and our method in detail. As shown in Figure 6, we visualized the positioning errors of GVINS and our proposed method. We see that the results of GVINS and our system fluctuate within a small range in the ENU frame and ECEF frame. The positioning errors of our method are less than 1m among all three directions of the ENU frame. Compared with GVINS, the error of our system is smaller in the east and north directions. In addition, the error of our method is smooth and stable in the ECEF frame, which is because we smooth the pseudorange measurements with the carrier phase. In addition, we listed the RMSE and Spherical Error Probable (SEP) for the positioning error of GVINS and our method in the ENU frame and ECEF frame. From

Table 1. RMSE[m] and SEP[m] statistic of GVINS and our proposed method in the sports field.

	ENU Frame				ECEF Frame
	RMSE			SEP	RMSE			SEP
	East	North	Up		X	Y	Z
GVINS	0.661	0.477	0.296	0.683	0.834	0.766	0.581	1.002
Proposed	0.524	0.360	0.725	0.761	0.522	0.440	0.466	0.651

, we can see that the RMSE of our positioning errors is lower than that of GVINS except in the up direction. The SEP refers to the radius of a sphere in which 50% of the estimated positions occur, and it can also evaluate the positioning precision of our system. The smaller the SEP, the higher the positioning precision of our system. We see that the SEP of our method in the ECEF frame is lower than that of GVINS, which shows that the positioning precision of our method is more accurate in the ECEF frame.

4.2. Complex Environment

In the following, we conducted experiments on the dataset complex environment, and we compared VINS-Mono, GVINS, RTK, and our proposed method. As shown in Figure 7, since VINS-Mono has four unobservable directions, the trajectory of VINS-Mono has a large drift. In addition, we saw that a section of the trajectory of VINS-Mono deviated from RTK by about 34 m, which is an unacceptable result. On the contrary, since GVINS and our proposed system are tightly coupled GNSS-VIO systems, there is no accumulated drift theoretically. In addition, due to a section of the trajectory being collected indoors, the GNSS receiver cannot obtain measurements at this time, which causes the failure of the RTK solution. In contrast, even if the GNSS measurements cannot be obtained, our system works well with VIO, and our trajectory is still accurate.

To further compare the performance of GVINS and our method, we analyzed the positioning errors of the two methods in the dataset complex environment as shown in Figure 8. It should be noted that we compared the RTK solution with our method as the ground truth. However, when the receiver is occluded, the satellite signal cannot be received, and the RTK solution is useless at this time. Thus, we only compared with segments where the RTK solution is available. Due to the complexity of the dataset, the overall positioning precision of the system is inferior to that of the sports field. The RMSE and SEP of GVINS and our method are shown in

Table 2. RMSE[m] and SEP[m] statistic of GVINS and our proposed method in the complex environment.

	ENU Frame
	RMSE			SEP
	East	North	Up
GVINS	1.091	0.782	3.016	2.540
Proposed	1.230	1.404	2.499	2.390

. We see that the positioning errors of GVINS in the east and north directions are slightly lower than that of our method, while the positioning error of our method in the up direction is much lower than that of GVINS. The SEP metric also shows that our method outperforms GVINS on this dataset.

Furthermore, since the ENU frame is gravity-aligned, we further analyzed the 2D trajectory error in the East–North plane as shown in Figure 9. The trajectory projected on the East–North plane is stained in red. The z-axis represents the positioning errors in the corresponding East–North plane, and the errors are represented by a scatter plot. In some bright or dim scenes, only a few point and line features can be detected, which will affect the positioning precision of the system. In addition, buildings, indoor environments, and trees will block satellite signals, which also degrade the performance of our system. Therefore, the positioning error in the complex environment is larger than that in the sports field, and the maximum error exceeds 2 m. As depicted in Figure 9, a sudden increase in positioning error occurs at the curves of the trajectory due to the large change in the orientation direction. On the contrary, the positioning error of the trajectory with small curvature is smoother compared with that of the trajectory with large curvature, which shows that our method is still effective even in complex scenes.

4.3. Observability Analysis

In Section 2.7, we demonstrated that the states represented in the ECEF are fully observable. However, according to [20], if the states are represented in the local world frame, the GNSS-VIO system has still four unobservable directions. Therefore, we can perform observability analysis from experimental results. Figure 10 shows the 3D trajectories of the complex environment represented in the ECEF frame and local world frame. We zoomed in on the trajectories of turning and climbing stairs for the convenience of the analysis. From Figure 10b, we see that since the states are unobservable in the local world frame, the two trajectories of climbing the same stairs have a drift. Even if the GNSS measurements are included in our system, the two trajectories are still inconsistent. In contrast, due to the states represented in the ECEF frame being fully observable, the two trajectories of climbing the same stairs are consistent and very close. Similarly, the same conclusions can be obtained for the trajectories of turning as shown in Figure 10. According to the results of the observability analysis, there is an obvious drift in the two trajectories of turning in the local world frame, while the phenomenon does not occur in the ECEF frame. In addition, the consistency of the system also shows that even in large-scale scenarios, it can still achieve high-precision positioning results as illustrated in Figure 10a.

4.4. Smoothed Interval

Equation (15) requires the receiver to continuously lock the carrier phase. If the receiver loses lock, the smoothed pseudorange is corrupted by any cycle slip that occurs and must be reinitialized when that happens. Therefore, it is necessary to investigate the influence of different smoothed intervals on the positioning precision of our system. The positioning error with six different settings is illustrated in Figure 11. When the smoothed interval is set to 200 epochs, the system has the largest positioning error in the three directions of the ENU frame. This happens because the receiver loses lock frequently within a smoothed interval and then cycle slip occurs, which causes an increase in the positioning error. By reducing the smoothed interval, the phenomenon that the receiver loses lock can be eliminated, which can improve the positioning precision of the carrier phase smoothed pseudorange. Obviously, with the smoothed intervals of 5, 10, 20, and 50 epochs, our system has smaller errors in east and north directions compared to the smoothed intervals of 100 or 200 epochs. However, if the smoothed interval is too small, the positioning precision in up direction cannot be further improved, and may even become worse. As shown in Figure 11, when the smoothed interval is set to 5 epochs, the positioning error in the up direction is the largest among different settings. The reason for this phenomenon is that when the smoothed interval is too small, the carrier phase cannot smooth the pseudorange well, which causes the smoothed pseudorange measurement to degenerate into the pseudorange measurement.

4.5. Line Feature Tracking Threshold

In the nonlinear optimization stage, if the number of keyframes for which line features are continuously observed is less than a threshold, then these line features are filtered out. A different threshold has a great impact on the robustness and positioning precision. As shown in Figure 12, a threshold of 2 means that those line segments that are observed by two consecutive keyframes or more can be added to the factor graph. However, since these line segments are continuously tracked for too few frames, they are not stable enough for nonlinear optimization, which will definitely reduce the positioning precision. On the contrary, if the line feature tracking threshold varies between 3 and 4, the proposed system can obtain more accurate positioning results in the ENU frame. Because the line segments observed by three or four consecutive keyframes are more stable and more suitable for nonlinear optimization.

5. Conclusions

In this paper, we propose a tightly coupled GNSS-VIO system based on point-line features. First, since line features contain more environmental structure information, we added line features to the system. Second, the pseudorange measurement can only achieve meter-level positioning precision but is fast and unambiguous. On the contrary, the carrier phase measurement can achieve the centimeter-level positioning precision but is required to solve the whole-cycle ambiguity, which is time-consuming. Therefore, we propose to combine the advantages of the two measurements and replace the pseudorange with the carrier phase smoothed pseudorange, which can greatly improve the performance of our system. Third, we considered the extrinsic translation parameter between the GNSS receiver and IMU, and our system can perform real-time parameter calibration. Finally, we demonstrated that if the states are represented in the ECEF frame, the tightly coupled GNSS-VIO system is fully observable. We conducted experiments on public datasets, which show our system achieves high-precision, robust, and real-time localization. In the future, we will further focus on improved methods for tightly coupled GNSS-VIO systems.