Sky-GVIO: an enhanced GNSS/INS/Vision navigation with FCN-based sky-segmentation in urban canyon

Sky-view images can be significantly affected by factors such as clouds and lighting conditions, making it challenging to achieve high-precision segmentation using traditional methods based on pixel [29], category [30], region [31], and so on. It is well-known that FCN represent a mature pixel-level semantic segmentation network [32]. The FCN network structure primarily consists of two parts: the fully convolutional part and the deconvolution part. The fully convolutional part comprises classical CNN networks, such as VGG and ResNet, which are used for feature extraction. The deconvolution part, on the other hand, upsamples the feature maps to obtain the original-sized semantic segmentation image.

In this paper, the existing ResNet50 [33] is used for downsampling, which includes 48 convolutional layers. The deconvolution part, on the other hand, upsamples the feature maps to obtain the original-sized semantic segmentation images. In this paper, the upsampling is based on FCN-8s. In the upsampling process, FCN-8s uses transposed convolution to scale the 8x, 16x and 32x feature maps to the original size, and combines these three scaled feature maps by introducing skip connection, so as to ensure the learning of features at different scales.

The input of FCN can be any size images, the output is the same size as the input, and the number of channels is n (number of target categories) +1 (background). For the sky-view images segmentation task, two types of labels are required (sky region and non-sky region), so the number of channels for sky-view images segmentation algorithm based on FCN is 2. In addition, in this study, we made 440 training datasets by ourselves. As shown in Fig. 2, we built a sky-view images segmentation model based on FCN.

GNSS model, INS dynamic model and visual observation model are introduced, respectively. Subsequently, the state model and measurement model of Sky-GVIO integration model are described. Finally, we use the segmentation results to realize the NLOS detection and construct the LOS/NLOS model for NLOS mitigation.

1) GNSS Observation Model: The original pseudorange and carrier phase observation equations in GNSS positioning are expressed as follows:

where $P$ and $L$ represent the pseudorange and carrier phase, respectively. The angular symbols $s$ and $r$ refer to satellites and receivers, respectively. $\rho$ denotes the geometric distance between the phase centers of the receiver and satellite antennas. $t_{r}$ and $t^{s}$ respectively represent receiver and satellite clock offsets. The speed of light is $c$. $I$ and $T$ refer to the ionospheric and troposphere delay, respectively. $\lambda$ represents the carrier wavelength. $N$ represents carrier phase ambiguity. $\varepsilon_{P}$ and $\varepsilon_{L}$ represent pseudorange noise and carrier phase noise, respectively.

For SPP model, equation (1) is sufficient. However, in the case of RTK model, it can be represented as follows:

where $\nabla\Delta$ denotes the double-differenced (DD) operator. The DD operation is used to not only eliminate satellite orbit errors and clock errors but also to mitigate receiver clock errors, tropospheric and ionospheric delays, making it a powerful technique in GNSS positioning.

2) INS Dynamic Model: Considering the noisy measurement of the low-cost IMU, the Coriolis and centrifugal forces due to earth rotation are ignored in the IMU formulation. The inertial measurement can be modeled [34] in $b$ (body) frame as follows:

where $\left[\tilde{\bm{a}}_{k},\tilde{\bm{\omega}}_{k}\right]$ is the output of the IMU at time $k$ and $\left[\bm{a}_{k},\bm{\omega}_{k}\right]$ is the linear acceleration and angular velocity of the IMU sensor. $\bm{b}_{a_{k}}$ and $\bm{b}_{\omega_{k}}$ respectively are the biases of the accelerometer and gyroscope at time $k$ . In addition, $\bm{n}_{a}$ and $\bm{n}_{\omega}$ are assumed to be zero-mean Gaussian distributed with $\bm{n_{a}}\sim N\left(0,\Sigma_{n_{a}}\right)$ , $\bm{n_{\omega}}\sim N\left(0,\Sigma_{n_{\omega}}\right)$. $\bm{R}^{n}_{{b}_{k}}$ denotes the rotation matrix from IMU body ($b$)-frame to navigation ($n$)-frame. $\bm{g}^{n}$ is the gravity in the $n$ frame.

The linearized INS dynamic model [35] can be expressed as:

where $\delta\bm{\dot{\theta}}$, $\delta\dot{\mathbf{v}}^{n}$ and $\delta\dot{\mathbf{p}}^{n}$ represent the derivative of attitude, velocity and position errors in $\mathit{n}$ frame, respectively. The derivatives of $\delta\dot{\mathbf{b}}_{a}$ and $\delta\dot{\mathbf{b}}_{\omega}$, denoting the accelerometer and gyroscope biases in $\mathit{b}$ frame, respectively. In addition, $\mathbf{R}_{b}^{n}$ represents the rotation matrix from $\mathit{b}$ frame to $\mathit{n}$ frame; $\bm{\tilde{\omega}}$ and $\tilde{\bm{a}}$ represent the outputs of gyroscope and accelerometer, respectively; $\bm{b}_{\omega}$ and $\bm{b}_{a}$ represent the nominal biases of gyroscope and accelerometer, respectively; $\delta\bm{\theta}$ and $\delta\mathbf{v}^{n}$ represent the errors of attitude and velocity in $\mathit{n}$ frame, respectively; $\delta\bm{b}_{\omega}$ and $\delta\bm{b}_{a}$ represent the errors of gyroscope bias and accelerometer bias, respectively; $\bm{n}_{\omega}$ and $\bm{n}_{a}$ represent the noises of angular rate and acceleration, respectively; $\mathbf{n}_{b_{a}}$ and $\mathbf{n}_{b_{\omega}}$ represent the noises of gyroscope bias and accelerometer bias, respectively. The symbol $\left(\cdot\right)^{\wedge}$ is the cross-product.

Therefore, the error state vector of INS can be expressed as:

3) Visual Measurement Model: The core idea of the well-known MSCKF is to establish geometric constraints between multi-camera states by utilizing the same visual feature points observed by multi-cameras. Following this concept, we establish a visual model. For a visual feature point $f^{j}$ observed by a stereo camera at time $i$, its visual observation model [34] on the normalized projection planes of the left and right cameras can be represented as follows:

where the subscripts 0 and 1 represent the left and right cameras, respectively. $\left(\mathit{u}_{c_{0,j}}^{j},\mathit{v}_{c_{0,j}}^{j}\right)^{T}$ and $\left(\mathit{u}_{c_{1,j}}^{j},\mathit{v}_{c_{1,j}}^{j}\right)^{T}$ are the pixel coordinates of the same feature point on the normalized plane for left camera and right camera, respectively. $\mathit{\epsilon}_{cam,i}^{j}$ is visual measurement noise. $\left(\mathit{X}_{c_{0,j}}^{j},\mathit{Y}_{c_{0,j}}^{j},\mathit{Z}_{c_{0,j}}^{j}\right)^{T}$ and $\left(\mathit{X}_{c_{1,j}}^{j},\mathit{Y}_{c_{1,j}}^{j},\mathit{Z}_{c_{1,j}}^{j}\right)^{T}$ represents the position of the same feature point for left camera and right camera in $\mathit{c}$ frame, which can be expressed as:

where $\mathbf{R}_{c_{0,i}}^{c_{1,i}}$ and $\bm{p}_{c_{0,i}}^{n}$ are the rotation matrix and position of the left camera at time $\mathit{i}$ in $\mathit{n}$ frame, respectively. $\mathbf{R}_{c_{0,i}}^{c_{1,i}}$ is the rotation matrix from left camera to right camera at time $\mathit{i}$, $\bm{p}_{c_{1,i}}^{c_{0,i}}$ is the translation matrix from left camera to right camera, which can be accurately corrected in advance [36]. $\bm{p}_{j}^{n}$ and $\bm{p}_{j}^{c_{0,i}}$ respectively are the positions of the same visual feature point in $\mathit{n}$ frame and left $\mathit{c}$ frame.

We adopted the method proposed by [37] to construct the visual reprojection error between relative camera poses, and the visual state vector was described as:

where $\delta\bm{\theta}^{n}_{c_{i}}$ and $\delta\bm{p}^{n}_{c_{i}}$ are attitude errors and position errors at time $\mathit{i}$. The subscript $\mathit{s}$ represents the total number of camera poses in the sliding window. The measurement equation of visual reprojection error is expressed as follows:

$\bm{\tilde{z}}_{cam}$ and $\bm{\hat{z}}_{cam}$ represent visual observations and visual reprojection observations, respectively; $\bm{H}_{cam}$ represents the Jacobi matrix of stereo camera positioning model.

4) State and Measurement model of the Tightly Coupled GNSS/INS/Vision: This paper employs MSCKF for the tightly coupled GNSS/INS/Vision integration. Based on the above introductions of different sensor models, the complete state model for the tightly coupled GNSS/INS/Vision integration is as follows:

For both SPP and RTK positioning modes, this paper has constructed state models separately:

where $\delta\nabla\Delta N$ represents the DD carrier phase ambiguity.In addition, the error state model of INS and vision have already been provided in equation (7) and (11).

The state prediction model for the tightly coupled GNSS/INS/Vision integration is as follows:

where $\bm{F}_{ins}$ is the system matrices of INS state which could be directly from equation (6). $\bm{n}_{ins}$ and $\bm{n}_{GNSS}$ are the process noises of INS and map, respectively. In addition, the camera poses in the sliding window are considered constant, so its process noise is $\mathbf{0}$. Based on equation (6), the special form of can be written as:

where $\mathbf{I}$ is the identity matrix.

To deal with discrete time measurement from the INS, the 4th order Runge-kutta [7] numerical integration of equation (17) to propagate the estimated state variables. It is worth noting, only the IMU state variables are propagated, the visual and GNSS state variables are only copied. Meanwhile, we also need to propagate the covariance of the state:

where $\bm{\Phi}_{k,k-1}$ represents the discrete state transition matrix, $\bm{F}(\tau)$ is the continuous time state transition matrix at time $\tau$ ($\tau\in\left(t_{k},t_{k+1}\right)$) and $\bm{Q}_{k-1}$ is the discrete time noise covariance. $\bm{P}_{k-1}$ represents the error state covariance matrix before augmentation. $\bm{P}_{k,k-1}$ represents the one-step prediction error covariance matrix from time $t_{k-1}$ to time $t_{k}$.

It is worth noting that every time a new image is recorded, the state and covariance matrix will be augmented with a copy of the current camera pose estimate. The initial value of camera pose is derived from the INS mechanization and the covariance matrix $\bm{P}_{k}$ after augmented can be expressed as:

where $y$ and $m$ represent the number of variables related to GNSS and vision at a certain moment. And when the GNSS is recorded, we only need to remove and add state variables and corresponding covariance. $\mathbf{J}$ is the Jacobi matrix, which has the following form:

where $\mathbf{R}_{c}^{b}$ and $\bm{p}_{c}^{b}$ are the rotation matrix and translation matrix between camera and IMU, which are calibrated offline [36].

Based on the previous equations, the measurement equation for the tightly coupled SPP/INS/Vision integration are formulated as follows:

where $\delta\bm{P}_{SPP}$ is error of pseudorange observation in SPP and $\delta\bm{z}_{cam}$ is error of visual observation in equation (12). ${\bf H}_{P,SPP}$ is the Jacobi matrix of pseudorange error and ${\bf H}_{cam}$ is the Jacobi matrix of the involved camera states in equation (12). Then $\delta\bm{x}_{ins}$, $\delta\bm{x}_{GNSS,SPP}$ and $\delta\bm{x}_{cam}$ are the error state vectors of INS, SPP and Vision which can be found in equation (7), (14) and (11), respectively. In the same way, $\bm{\varepsilon}_{P,SPP}$ and $\bm{\varepsilon}_{cam}$ denote pseudorange observation error noise in SPP and visual observation error noise, respectively. In addition, $P$ and $\bm{\hat{P}}_{ins}$ are the actual measured pseudorange in equation (1) and the pseudorange predicted by INS mechanization, respectively. $\bm{z}_{cam}$ and $\hat{\bm{z}}_{cam}$ respectively represent the observed and reprojected visual measurements in equation (12).

The measurement equation for the tightly coupled RTK/INS/Vision integration are formulated as follows:

where $\delta\bm{P}_{RTK}$ and $\delta\bm{L}_{RTK}$ represent the observation errors of DD pseudorange and DD carrier phase in RTK, respectively. $\bm{\nabla\Delta P}$ and $\bm{\nabla\Delta L}$ can be found in equation (3). In addition, $\bm{\nabla\Delta\hat{P}}_{ins}$ and $\bm{\nabla\Delta\hat{L}}_{ins}$ are DD pseudorange predicted and DD carrier phase predicted by INS mechanization, respectively. Then ${\bf H}_{P,RTK}$ and ${\bf H}_{L,RTK}$ are the Jacobi matrices of DD pseudorange error and DD carrier phase error. $\delta\bm{x}_{GNSS,RTK}$ is the error state vector of RTK which can be found in equation (15). $\bm{\varepsilon}_{\nabla\Delta P,RTK}$ and $\bm{\varepsilon}_{\nabla\Delta L,RTK}$ denote DD pseudorange observation error noise and DD carrier phase observation error in RTK.

In equation (23), $f={1}/{\sin^{2}(ele)}$, $ele$ and $SNR$ refer to elevation angles and SNR of satellites, respectively. The work [38] gives $s_{1}=50$, $A=30$, $s_{0}=10$ and $a=20$, these parameters are empirical values. $\sigma_{P}$ and $\sigma_{L}$ represent the standard deviation of pseudorange and carrier phase respectively, which are 0.3 m and 0.03 m given in this paper. The $K$ is the scale factor, which is 10 in this paper. If the satellite’s projection is located in the sky semantic region, then Stochastic model of satellite observations will be modeled by using equation (24), and if not, it will be modeled by using equation (23).

For accurate modeling of the GNSS noise covariance in the tightly coupled GNSS/INS/Vision integration, it is essential to differentiate between LOS and NLOS satellites. Therefore, we obtain sky-mask after performing semantic segmentation of the sky-view images using FCN, and subsequently, based on the projection model mentioned in [28] and satellite information included elevation and azimuth angles provided by satellite ephemeris, we ultimately identify LOS and NLOS conditions around the GNSS receiver. Fig. 3 shows the overall flow of S-NDM algorithm. In this process, if the satellite’s projection is located in the sky region, then the satellite will be classified as an LOS satellite (represented by a blue dot), and if not, it will be classified as an NLOS satellite (represented by a red dot). Through this satellite visualization strategy, we can obtain the judgment conditions of equation (23) and (24). Different from judging conditions by experience threshold in [38], satellite visualization strategy is more reliable, which is also the difference between LOS/NLOS signal modeling in this paper and traditional methods.