Adaptive Weighted Factor Graph Optimized Positioning Algorithm Based on Joint GNSS/INS/Vision Residual Detection

Abstract

Multi-sensor fusion of GNSS, IMU, and vision sensors has been extensively applied in urban Internet of Things systems and automated driving to improve positioning accuracy in complex environments. However, conventional FGO algorithms are based on fixed sensor weights, which limit their adaptability to fluctuations in sensor errors caused by environmental changes, thereby compromising positioning performance. To overcome this limitation, a novel multi-sensor adaptive weighted localization algorithm based on joint residuals detection was proposed in this study. The algorithm computes joint residuals by the sliding window accumulation of GNSS, IMU, and vision sensor measurements. By integrating a global weight decay factor into the M-estimation framework, the weights of each sensor were dynamically adjusted, thereby suppressing the effects of outliers on the state estimation. This approach enables high-precision and robust estimation of position, velocity, and attitude. Experimental results demonstrate that, based on validation with the GNSS–Visual–Inertial Navigation System (GVINS) public datasets sports field and complex environments, the proposed method exhibits superior performance in challenging low-altitude economic scenarios such as weak GNSS signals and significant IMU drift—specifically, it improves positioning accuracy by 32.3% and reduces velocity error by 32% compared to traditional FGO algorithms. In scenarios with GNSS signal interference, the system effectively mitigates error accumulation and maintains the stability of position and velocity estimation. The proposed algorithm demonstrates exceptional positioning accuracy and robustness in complex and dynamic environments, making it highly suitable for advanced urban IoT and automated driving applications.

1. Introduction

In modern navigation and positioning systems, obtaining high-precision positional information is essential for applications such as autonomous driving, smart cities, and unmanned aerial vehicles. However, conventional positioning methods, including global navigation satellite systems and INS, have significant limitations in complex environments. GNSS signals are highly susceptible to disruptions in challenging scenarios such as urban canyons, tunnels, and densely forested areas, and inertial measurement unit errors accumulate over time, resulting in a rapid decline in positioning accuracy. To overcome these challenges, GNSS are frequently integrated with inertial measurement units and vision sensors to leverage their complementary strengths and improve navigation and localization performance [1,2,3].

The Kalman filter and its extended variants are widely used to combine GNSS, IMU, and vision sensors in various navigation and localization systems. For example, Li et al. integrated GNSS, INS, and vision sensors using a Kalman filter-based framework, whereas the Rauch–Tung–Striebel smoother, also known as the forward–backward smoother, was used to achieve optimal attitude estimation [4]. Similarly, Angelino et al. described a state-estimation system that fuses GNSS solutions with visual and inertial data within an EKF framework [5]. Furthermore, Shen et al. presented a traceless Kalman filter algorithm that combines visual, inertial, LiDAR, and GNSS data to generate smooth and consistent trajectories in different environments [6]. Building on these advances, Sun et al. proposed a robust two-step integration algorithm based on an EKF. In this method, IMU, GPS, and vision sensor data are fused using incomplete constraints to improve the vehicle state estimation in diverse urban scenarios [7]. Although the Kalman filter and its variants are effective, they have inherent problems in handling nonlinear errors. These methods rely solely on the current state and do not retain information from past states, making it impossible to leverage past data for releasing or iterative optimization. Consequently, their ability to improve estimation accuracy of the current state is inherently limited [8].

To overcome these limitations and better handle nonlinear models while leveraging a batch of measurements for improved accuracy, FGO methods have gained significant traction in recent years [9,10,11]. The FGO represents multi-sensor systems as factor graph models, where the optimal state estimate is derived by computing the maximum a posteriori estimate of the joint probability distribution, which is the foundation of the approach presented in this work. Compared to the conventional Kalman filter methods, FGO can provide improved accuracy in practical state estimation when dealing with nonlinear models. This is because the batch optimization process in FGO allows for multiple iterations and relinearization of measurement models within the sliding window, which can converge to a more optimal solution compared to the single-step update in EKF, especially when initial state estimates are poor or models are highly nonlinear. For example, Ma et al. presented a multi-sensor fusion navigation framework based on the FGO, which integrates tightly coupled visual information from optical arrays [12]. This approach enables high-precision, high-frequency relative position estimation between an autonomous underwater vehicle and its moving dock during the terminal docking phase. Similarly, Mascaro et al. proposed a multi-sensor fusion method that uses decoupled graph optimization to estimate the global 6-DOF attitude of a robot in real time [13]. This method combines a generic 6-DOF visual–inertial odometry attitude with a 3-DOF global reference position for robust and efficient localization. The FGO demonstrated exceptional performance in dynamic environments. For example, Li et al. emphasizes the use of factor graphs for tightly coupled vision and IMU data fusion [14]. This approach not only achieves highly accurate position estimation, but also provides more continuous localization information under unstable GNSS signal conditions by utilizing vision data. In addition, to address the susceptibility of visual navigation systems to complex environmental conditions, Niu et al. presented a robust real-time GNSS–visual–inertial navigation system centered on inertial navigation systems [15]. By exploiting the inherent resilience of the INS to external environmental factors, this system maximizes the advantages of the INS in terms of improving navigation performance.

However, existing FGO methods are often designed based on static data processing windows or fixed optimization strategies that do not effectively address the challenges posed by data variations in real-time environments. In particular, dynamic environments lead to significant changes in the sampling frequency and noise characteristics of sensor data, resulting in a decline in localization accuracy if conventional methods cannot adapt to these changes [16].

To solve this problem, Ben et al. proposed a robust FGO method based on dynamic kernel principal component analysis [17]. This method uses a time-lag matrix to extract nonlinear and Markov features from the incremental navigation data. In addition, a performance-monitoring system was developed to detect and evaluate faults in real time to ensure higher reliability and adaptability. Similarly, Zhou et al. derived and constructed a robust factor graph based on an improved chi-square test [18]. This approach integrates a robust factor graph with an improved chi-square test to optimize the performance of the integrated navigation algorithm to ensure greater resilience and accuracy under dynamic and uncertain conditions.

The factor graph method typically relies on empirically determined noise variance matrices to assign weights to observations, a practice that aligns with the BLUE framework, which assumes fixed noise characteristics. However, in practical applications, observation information is subject to uncertainty due to environmental dynamics, leading to fluctuations in noise variance. This violates the static assumptions of BLUE, resulting in a weighting process overly dependent on initial empirical values and incapable of dynamic adjustment [19,20]. Usually, sensors with higher empirical accuracy are assigned larger weights [21]. However, even if the performance of a particular sensor degrades significantly during data fusion, its weights remain static, thereby compromising the overall fusion performance [22]. To address these challenges, An adaptive weighting strategy based on a chi-square test was presented by Sun et al. [23]. Their approach evaluates the data quality of the GNSS and visual sensors in real time and dynamically adjusts their weights accordingly. This strategy shows strong performance in complex urban environments and significantly improves positioning accuracy.

However, the chi-square test is very sensitive to assumptions and sample sizes, which may lead to insufficient statistical effectiveness if the sample size is too small or inflated owing to the amplification of minor differences with large sample sizes. Based on this, Zhou et al. proposed a robust factor graph-optimized combination navigation algorithm that incorporates an improved chi-square test [24,25]. By incorporating a down-weighting strategy from the M-estimator, the binary results of the chi-square test were refined to increase stability and reduce false positives. Despite these improvements, the down-weighting strategy in this system remains overly simplistic and limits its ability to account for more diverse and complex scenarios.

To overcome the limitations of existing sensor weighting strategies, which are overly simplistic and perform poorly in complex and dynamic environments, and to address the lack of accurate and reliable sensor quality assessment mechanisms, this an adaptive weighted factor graph-optimized positioning algorithm based on joint GNSS/IMU/vision residual detection was proposed in this study. The main contributions of this paper can be summarized as follows:

(1)

To provide robust support for subsequent integrated navigation, this study proposes an adaptive weighted factor graph optimized positioning algorithm based on joint GNSS/IMU/vision residual detection. This approach uses joint residual analysis to dynamically evaluate the quality of sensor data, enabling real-time adjustments in sensor weight assignment and ensuring improved accuracy and reliability in combined navigation systems.

(2)

A proportional factor model is proposed, leveraging the joint residual test statistics of observation data and M-estimation as an adaptive weighted fusion strategy for GNSS, IMU, and vision measurements. This approach effectively improves the positioning accuracy and robustness of multi-sensor integrated navigation systems, particularly in complex and dynamic environments.

The rest of the article is organized as follows: In the second part, an overview of the system and the FGO algorithm is presented. In the third section, the proposed method is described in detail. In the fourth part, simulation experiments are carried out in different scenarios to verify the effectiveness of the proposed method. Finally, the fifth part draws conclusions from this study.

2. Overview

2.1. System Overview

A system block diagram of the proposed multi-sensor weighted fusion algorithm based on observation data residuals is illustrated in Figure 1. The algorithm consists of three primary phases. (1) The system collects preliminary position data from multiple sensors. The GNSS module receives satellite signals to generate pseudo-range measurements, the IMU module processes acceleration and angular velocity data and performs pre-integration to provide motion-related information, and the vision module extracts feature points and computes the positioning information using triangulation techniques. (2) The system evaluates the residuals of the observed data from the GNSS, IMU, and vision sensors to assess the data reliability. A test statistic is calculated and compared against a preset threshold value. If the test statistic is smaller than the threshold, the data is classified as reliable and included directly into the factor graph optimization process. If the test statistic exceeds the threshold, the data is identified as abnormal, and a down-weighting strategy is applied to minimize their influence on the fusion results. (3) Finally, the system performs factor graph optimization to fuse the multi-sensor data and generate accurate state estimation results through joint optimization, including precise position information and attitude solutions. This stepwise process ensures that the proposed algorithm effectively identifies and mitigates the effects of abnormal data, while leveraging the complementary strengths of the GNSS, IMU, and vision sensors to achieve accurate and robust positioning.

2.2. Frame Definition

Figure 2 illustrates the relationships between the different coordinate systems used in this study. The color scheme is defined as follows: red represents the Local World Frame ${(·)}^w$, black represents the coordinate system framework, and blue represents the East–North–Up (ENU) frame. The specific definitions are as follows:

(1)

Sensor Frame: The sensor coordinate system mainly includes the camera frame ${(·)}^c$ and the IMU frame ${(·)}^i$. In this paper, the IMU frame is chosen as the carrier body frame ${(·)}^b$.

(2)

Local World Frame: The visual–inertial system running frame is localized as the world frame ${(·)}^w$, as shown in Figure 2. The origin of this frame is arbitrary, and the Z-axis is aligned with the direction of gravity.

(3)

Earth-Centered, Earth-Fixed (ECEF) Frame: The ECEF frame is an Earth-centered fixed coordinate system whose origin is located at the Earth’s center of mass. The X-axis points to the intersection of the equator and the prime meridian. The Y-axis lies in the equatorial plane, perpendicular to the X-axis, pointing towards ${90}^{\circ }$ east longitude. The Z-axis points towards the North Pole along the Earth’s rotation axis.

(4)

ENU Frame: To connect the local world coordinate system and the ECEF frame, the ENU frame is adopted in this paper. Its X, Y, and Z axes point east, north, and upward, respectively.

3. Methodology

In this study, a factor graph model was constructed by representing the residuals of the GNSS, IMU, and vision sensors as a factor, respectively. The best system state estimation is obtained by maximizing the a posteriori probability when all sensor measurements are known:

$$\begin{array}{cc}\hfill X^{*}& =\underset{X}{\mathrm{argmax}}p\left(X\right|Z)\hfill \\ \hfill & =\underset{X}{\mathrm{argmax}}p\left(X\right){\displaystyle \prod _{i=1}^n}p\left(Z_i\right|X)\hfill \\ \hfill & =\underset{X}{\mathrm{argmin}}\left\{{∥{\phi }_p-H_{p}X∥}^2+{\displaystyle \sum _{i=1}^n}{∥\phi (Z_i,X)∥}_{P_i}^2\right\}\hfill \end{array}$$

(1)

where X denotes the state of the system and Z represents the set of n observations. ${\phi }_p$ and $H_p$ are the a priori information about the system, $\phi (·)$ denotes the residual function for each measurement, and $P_i$ is the Mahalanobis paradigm. The core of our method lies in the dynamic adjustment of the weighting implied by $\sum _i$ based on real-time residual analysis.

3.1. GNSS Factor

The raw pseudorange measurement ${\tilde{\rho }}_r^s$ from satellite s to receiver r at epoch k is modeled as

$$\begin{array}{cc}\hfill {\tilde{\rho }}_r^s\left(k\right)& ={\rho }_r^s\left(k\right)+c\left(\delta t_r\left(k\right)-\delta t^s\left(k\right)\right)+T_r^s\left(k\right)+I_r^s\left(k\right)\hfill \\ \hfill & +M_r^s\left(k\right)+{\epsilon }_{\rho }\left(k\right)\hfill \end{array}$$

(2)

where ${\rho }_r^s\left(k\right)=∥P_r\left(k\right)-P^s\left(k\right)∥$ is the geometric distance between the satellite and the receiver; c is the speed of light in vacuum ($c\approx 3\times {10}^8$ m/s); $\delta t_r$ and $\delta t^s$ represent the receiver clock difference and the satellite clock difference, respectively; $T_r^s$ and $I_r^s$ represent the tropospheric and ionospheric delays, respectively; $M_r^s$ is the delay caused by the multipath effect, and ${\epsilon }_{\rho }$ represents the noise. Therefore, the pseudo-range residual can be expressed as

$${\phi }_{\rho }=∥P_{s_j}-P_{r_k}∥+c\left(\delta t_k-\delta t_{s_j}\right)+T_{r_k}^{s_j}+I_{r_k}^{s_j}-{\tilde{\rho }}_{r_k}^{s_j}$$

(3)

where $s_j$ denotes the Jth satellite and k denotes the moment.

3.2. Inertia Factor

Inertial measurements are modeled as follows [1]:

$$\begin{array}{cc}\hfill {\tilde{a}}_t& =a_t+b_{a_t}+n_a\hfill \\ \hfill {\tilde{\omega }}_t& ={\omega }_t+b_{{\omega }_t}+n_{\omega }\hfill \end{array}$$

(4)

where $\left\{{\tilde{a}}_t,{\tilde{\omega }}_t\right\}$ is the output of the IMU at time t, $\left\{b_{a_t},b_{{\omega }_t}\right\}$ is the zero bias of the accelerometer and gyroscope, and n is the noise. Then the relative position, velocity and rotation information $\left\{{\widehat{\alpha }}_{b_{t_{k+1}}}^{b_{t_k}},{\widehat{\beta }}_{b_{t_{k+1}}}^{b_{t_k}},{\widehat{\gamma }}_{b_{t_{k+1}}}^{b_{t_k}}\right\}$ from the $t_k$ moment to the $t_{k+1}$ moment can be obtained by pre-integrating the IMU. Therefore, the measurement residuals of the IMU can be expressed as follows [1]:

$$\begin{array}{cc}\hfill {\phi }_I& =\left[\begin{array}{c}\delta {\alpha }_{b_{t_{k+1}}}^{b_{t_k}}\\ \delta {\beta }_{b_{t_{k+1}}}^{b_{t_k}}\\ \delta {\theta }_{b_{t_{k+1}}}^{b_{t_k}}\\ \delta b_a\\ \delta b_g\end{array}\right]\hfill \\ \hfill & =\left[\begin{array}{c}\left(p_{b_{t_{k+1}}}-p_{b_{t_k}}+{\displaystyle \frac{1}{2}}g\Delta t_k^2-v_{b_k}\Delta t_k\right)-{\widehat{\alpha }}_{b_{t_{k+1}}}^{b_{t_k}}\\ \left(v_{b_{t_{k+1}}}+g\Delta t_k-v_{b_{t_k}}\right)-{\widehat{\beta }}_{b_{t_{k+1}}}^{b_{t_k}}\\ 2{\left[q_{b_{t_k}}^{-1}\otimes q_{b_{t_{k+1}}}\otimes {\left({\widehat{\gamma }}_{b_{t_{k+1}}}^{b_{t_k}}\right)}^{-1}\right]}_{xyz}\\ b_{a{b}_{t_{k+1}}}-b_{a{b}_{t_k}}\\ b_{\omega b_{t_{k+1}}}-b_{\omega b_{t_k}}\end{array}\right]\hfill \end{array}$$

(5)

where $\delta {\theta }_{b_{t_{k+1}}}^{b_{t_k}}$ is the rotation error, v is the velocity, p is the position, q is the attitude, and $b_{t_k}$ is the carrier frame at the moment $t_k$, ${\left[·\right]}_{xyz}$ returns the imaginary part of the quaternion.

3.3. Visual Factor

The visual projection process is modeled as follows:

$$\tilde{\mathrm{P}}={\pi }_c\left(R_b^c\left(R_w^b{X}^w+p_w^b\right)+p_b^c\right)+n_c$$

(6)

where $\tilde{P}$ is the coordinate of the feature point in the image plane and $X^w$ is the corresponding 3D coordinate in the local world coordinate system. ${\pi }_c\left(·\right)$ is the projection function of the camera, and $n_c$ is the measurement noise, $R_b^c$ is the rotation matrix from the body frame to the camera frame, and $R_w^b$ is the rotation matrix from the world frame to the body frame. Therefore, if a feature l with an inverse depth $\rho l$ in frame i is observed again in frame j, the residual difference between the two corresponding frames can be expressed as

$${\phi }_c={\tilde{P}}_{c_{t_j}}-{\pi }_c\left({\widehat{X}}_{c_{t_j}}\right)$$

(7)

$$\begin{array}{cc}\hfill {\widehat{X}}_{c_{t_j}}& =R_b^c\left(R_w^{b_{t_j}}\left(R_{b_{t_i}}^w\left(R_c^b{\displaystyle \frac{1}{\rho l}}{\pi }_c^{-1}\left({\tilde{P}}_l^{c_{t_j}}\right)+p_c^b\right)+p_{b_{t_i}}^w\right)\right.\hfill \\ \hfill & \left.+p_w^{b_{t_j}}\right)+p_b^c\hfill \end{array}$$

(8)

where $\left\{R_c^b,t_c^b\right\}$ is the transformation matrix between the IMU and the camera coordinate system.

3.4. Sliding Window-Based Multi-Sensor Residual Accumulation with Adaptive Weighting Algorithm

The core innovation of our work is a two-layer adaptive weighting mechanism that combines global sensor reliability assessment with local observation robustness. In this system, a sliding window W is used to store the multi-sensor residuals for each epoch k, which includes the GNSS pseudo-range, IMU pre-integration, and position measurements from the vision sensors. When the sliding window receives the residuals from each sensor, they are processed sequentially and stored into the residual vector using a calendar element until the end of the sliding window. The following equation describes the process of accumulating multi-sensor residuals:

$$r_k=\left[r_{\rho }^{\left(k\right)},r_{IMU}^{\left(k\right)},r_{vision}^{\left(k\right)}\right]$$

(9)

where $r_k$ is the multisensor residual vector for the kth calendar element. $r_{\rho }^{\left(k\right)}$ is a GNSS pseudo-range residual that provides the geometric constraints between the receiver and the satellite. $r_{IMU}^{\left(k\right)}$ represents the IMU pre-integrated residuals used to estimate the position, velocity, and attitude changes between neighboring calendar elements. $r_{vision}^{\left(k\right)}$ is a visual sensor position residual that provides the relative position constraints between neighboring keyframes. The sliding window helps to smooth short-term errors and provides more stable state estimates for the system by accumulating residuals from multiple calendar elements.

(1)

GNSS Pseudorange Residuals

The GNSS pseudo-range measurement was realized by calculating the geometric distance between the receiver and the satellite. The pseudorange residual $r_{\rho ,i}^{\left(k\right)}$ in the kth calendar element represents the difference between the measured and theoretical values and is calculated as follows:

$$r_{\rho ,i}^{\left(k\right)}={\displaystyle \frac{{\widehat{\rho }}_i^{\left(k\right)}-\left(∥P_r^{\left(k\right)}-P_s^{\left(k\right)}∥+c\left(\delta t_r^{\left(k\right)}-\delta t_s^{\left(k\right)}\right)+T+I\right)}{{\sigma }_{\rho ,i}^{\left(k\right)}}}$$

(10)

where ${\widehat{\rho }}_i^{\left(k\right)}$ is the pseudorange measurement of the first satellite, $∥P_r^{\left(k\right)}-P_s^{\left(k\right)}∥$ is the geometric distance between the receiver and the satellite, and ${\sigma }_{\rho ,i}^{\left(k\right)}$ is the standard deviation of the pseudorange measurement. T and I represent the estimated tropospheric delay and ionospheric delay, respectively.

The pseudo-range residuals provide important geometric constraints in the optimization process, and by combining multiple satellite signals, the system can compute more accurate position information. However, GNSS signals are susceptible to multipath effects and signal occlusions, and therefore need to be combined with other sensor data to increase their robustness.

(2)

IMU Residuals

The IMU sensor measurements include acceleration and angular velocity, which can be used to estimate position, velocity, and attitude changes between neighboring calendar elements. To ensure that the data from different sensors have the same scale, the IMU residuals must be normalized. The specific calculation process is as follows.

The position residuals are used to represent the difference between the position changes between the neighboring calendar elements k and j and the IMU measurements, and the normalization process can be described as follows:

$$r_{pos,i,j}^{\left(k\right)}={\displaystyle \frac{P_j-\left(P_i+v_i\Delta t+\frac{1}{2}{R}_i{a}_i\Delta t^2\right)}{{\sigma }_{pos,i,j}^{\left(k\right)}}}$$

(11)

where $P_i$ and $P_j$ are the positions of the calendar elements i and j, respectively, $v_i$ is the velocity at the calendar element i, $R_i$ is the rotation matrix from the body frame ${(·)}^b$ to the world frame ${(·)}^w$, $a_i$ is the acceleration measurement, and ${\sigma }_{pos,i,j}^{\left(k\right)}$ is the standard deviation of the residuals of the IMU positions.

The velocity residual of the IMU represents the deviation in the velocity change between the two calendar elements and is normalized as follows:

$$r_{v,i,j}^{\left(k\right)}={\displaystyle \frac{v_j-\left(v_i+R_i{a}_i\Delta t\right)}{{\sigma }_{v,i,j}^{\left(k\right)}}}$$

(12)

where $v_i$ and $v_j$ denote the velocities of the calendar element i and j, respectively; ${\sigma }_{v,i,j}^{\left(k\right)}$ is the standard deviation of the velocity residuals.

The attitude residual represents the difference between the change in attitude between neighboring calendar elements and the IMU measurements. The normalization formula for the attitude residual is as follows:

$$r_{att,i,j}^{\left(k\right)}={\displaystyle \frac{\Delta q_{ij}-\Delta q_{IMU,ij}}{{\sigma }_{att,i,j}^{\left(k\right)}}}$$

(13)

where $\Delta q_{ij}$ is the quaternion of attitude change and ${\sigma }_{att,i,j}^{\left(k\right)}$ is the standard deviation of the attitude residuals. Through this normalization process, each residual quantity of the IMU is evaluated on the same scale, ensuring that the data from different sensors can be compared fairly.

(3)

Vision Sensor Residuals

The visual sensor tracks feature points in the image and calculates the relative positions between neighboring keyframes. Consistent with the tight coupling methodology described in Section 3.3, the visual residuals are defined as the reprojection error. Instead of using loose-coupled relative poses, we utilize the geometric constraint between the 3D map points and their 2D projections on the image plane. The visual residual for a feature l observed in frame j is calculated as

$$r_{\mathrm{vision},l,j}^{\left(k\right)}={\tilde{P}}_{l,j}^{\left(k\right)}-{\pi }_c\left({\widehat{X}}_{c_{t_j}}\right)$$

(14)

where ${\tilde{P}}_{l,j}^{\left(k\right)}$ is the observed 2D coordinate of feature l in frame j, and ${\widehat{X}}_{c_{t_j}}$ is the predicted projection derived from the state estimate X and the camera model (Equation (8)). This formulation ensures that the visual constraints are directly integrated into the factor graph, allowing the optimization to jointly refine the camera poses and landmark positions. The residuals are normalized as follows:

$$r_{\mathrm{vision},l,j}^{\left(k\right)}={\displaystyle \frac{{\tilde{P}}_{l,j}^{\left(k\right)}-{\pi }_c\left({\widehat{X}}_{c_{t_j}}\right)}{{\sigma }_{\mathrm{vision},l,j}^{\left(k\right)}}}$$

(15)

(4)

Calculation of Test Statistics Within a Sliding Window

Each calendar element in the sliding window can be evaluated using a test statistic to determine if the measurements are as expected. The test statistics within the sliding window are calculated as the sum of squared normalized residuals:

$$\begin{array}{cc}\hfill S_{\mathrm{GNSS}}& ={\displaystyle \sum _{i=1}^{N_s}}{\left({\displaystyle \frac{r_{\rho ,i}^{\left(k\right)}-{\mu }_k}{{\sigma }_k}}\right)}^2\hfill \\ \hfill S_{\mathrm{IMU}}& ={\displaystyle \sum _{j=1}^{N_{\mathrm{IMU}}}}{\left({\displaystyle \frac{r_{\mathrm{IMU},\mathrm{i},\mathrm{j}}^{\left(k\right)}}{{\sigma }_{\mathrm{IMU}}^{\left(k\right)}}}\right)}^2\hfill \\ \hfill S_{\mathrm{vision}}& ={\displaystyle \sum _{m=1}^{N_{\mathrm{vision}}}}{\left({\displaystyle \frac{r_{\mathrm{vision},l,j}^{\left(k\right)}}{{\sigma }_{\mathrm{vision}}^{\left(k\right)}}}\right)}^2\hfill \end{array}$$

(16)

Since $S_l$ is the sum of squares of standardized residuals, it theoretically follows a Chi-square (${\chi }^2$) distribution with degrees of freedom equal to the number of measurements $N_l$. However, to balance computational efficiency and robustness against non-Gaussian noise in dynamic environments, we employ an empirical heuristic threshold $T_l$ rather than a strict ${\chi }^2$ quantile:

$$\begin{array}{cc}\hfill T_{\mathrm{GNSS}}& ={\displaystyle \frac{\gamma }{N_{\mathrm{GNSS}}}}{S}_{\mathrm{GNSS}};\hfill \\ \hfill T_{\mathrm{IMU}}& ={\displaystyle \frac{\gamma }{N_{\mathrm{IMU}}}}{S}_{\mathrm{IMU}};\hfill \\ \hfill T_{\mathrm{Vision}}& ={\displaystyle \frac{\gamma }{N_{\mathrm{Vision}}}}{S}_{\mathrm{Vision}}.\hfill \end{array}$$

(17)

The threshold parameter $\gamma$ is empirically set to 2.0. This value has been validated through cross-validation on the GVINS dataset to effectively suppress outliers while maintaining sufficient measurement availability. We acknowledge that this threshold does not represent a rigorous 95% statistical confidence bound but serves as a practical engineering compromise. This value provides a balanced trade-off between robustness and sensitivity. When the test statistic $S_l$ exceeds the threshold $T_l$, the data is considered potentially unreliable, and a down-weighting strategy is triggered:

$${\omega }_l=\left\{\begin{array}{cc}1,\hfill & S_l\le T_l,\hfill \\ {\omega }_{l,i},\hfill & S_l>T_l\hfill \end{array}\right.$$

(18)

where l represents different sensors, if $S_l\le T_l$ indicates that the observation data of the current calendar element is of good quality, the current observation data can be trusted; if $S_l>T_l$ indicates that there may be outliers in the current observation data, the system needs to dynamically adjust the weights of different sensor types to minimize the influence of the outliers on the overall state estimation. At this point, a global weight attenuation factor ${\alpha }_l$ is introduced in this paper to adjust the weights of each sensor type in the calendar element:

$${\alpha }_l={\displaystyle \frac{T_l}{S_l}}$$

(19)

The global weight decay factor ${\alpha }_l$ was applied to the weights of each sensor, and the specific formula to update the weights is

$$\begin{array}{cc}\hfill {\omega }_{GNSS}& \leftarrow {\alpha }_l·{\omega }_{GNSS}^0\hfill \\ \hfill {\omega }_{IMU}& \leftarrow {\alpha }_l·{\omega }_{IMU}^0\hfill \\ \hfill {\omega }_{vision}& \leftarrow {\alpha }_l·{\omega }_{vision}^0\hfill \end{array}$$

(20)

where ${\omega }_{GNSS}^0,{\omega }_{IMU}^0,{\omega }_{Vision}^0$ is the initial weight and ${\omega }_{GNSS},{\omega }_{IMU},{\omega }_{Vision}$ is the updated weight. This adjustment ensures that all sensor weights are weakened when the test statistic is large, thereby reducing the effect of the calendar element observations on the overall state estimation. On this basis, M-estimation is introduced to further refine the residual processing within each sensor.

M-estimation is a robust estimation method that is widely used to deal with the presence of outliers in data. Unlike the conventional least-squares method, M-estimation makes the estimation more robust by choosing different loss functions to reduce the effect of outliers on the parameter estimation. The goal of M-estimation is to minimize the objective function as follows:

$$\widehat{\theta }=\underset{\theta }{argmin}{\displaystyle \sum _{i=1}^n}\rho \left(r_i\left(\theta \right)\right)$$

(21)

where $\rho \left(r\right)$ is the loss function and $r_i\left(\theta \right)$ is the residual function. To solve the problem of M-estimation, it is often necessary to use a weight function obtained from the derivative $\varphi \left(r\right)$ of the loss function:

$$\varphi \left(r\right)={\displaystyle \frac{d\rho \left(r\right)}{dr}}$$

(22)

A common Huber weighting function is described as

$$\phi \left(r\right)=\left\{\begin{array}{cc}1,\hfill & r\le \delta ,\hfill \\ {\displaystyle \frac{\delta }{\left|r\right|}},\hfill & r>\delta \hfill \end{array}\right.$$

(23)

This function assigns a lower weight to individual residuals with large normalized magnitudes. By combining the global weight decay factor ${\alpha }_k$ and the weight function estimated by M, the final weights can be expressed as follows:

$$\begin{array}{cc}\hfill {\omega }_{GNSS,i}& ={\alpha }_l·\phi \left({\displaystyle \frac{r_{GNSS,i}}{{\sigma }_{GNSS}}}\right)\hfill \\ \hfill {\omega }_{IMU,i}& ={\alpha }_l·\phi \left({\displaystyle \frac{r_{IMU,j}}{{\sigma }_{IMU}}}\right)\hfill \\ \hfill {\omega }_{vision,i}& ={\alpha }_l·\phi \left({\displaystyle \frac{r_{vision,i}}{{\sigma }_{vision}}}\right)\hfill \end{array}$$

(24)

The weights calculated in Equations (17)–(19) are not applied to the final state estimates directly. Instead, they are injected into the factor graph optimization framework defined in Equation (1) by dynamically scaling the information matrix of each sensor factor. In Equation (1), the term $\parallel \phi (Z_i,X){\parallel }_{P_i}^2$ represents the Mahalanobis distance $r^T\mathsf{\Lambda }r$, where $\mathsf{\Lambda }=P_i^{-1}$ is the information matrix. To implement adaptive weighting, the information matrix for sensor l is scaled by the global weight attenuation factor ${\alpha }_l$ and the M-estimation weight $\phi (·)$:

$${\mathsf{\Lambda }}_l^{\mathrm{adj}}={\alpha }_l·\phi \left({\displaystyle \frac{r_{l,i}}{{\sigma }_l}}\right)·{\mathsf{\Lambda }}_l^0$$

(25)

where ${\mathsf{\Lambda }}_l^0$ is the initial information matrix and $l\in \left\{\mathrm{GNSS},\mathrm{IMU},\mathrm{Vision}\right\}$. Consequently, the optimization problem becomes

$$X^{*}=\underset{X}{argmin}\left\{\parallel {\phi }_p-H_p{X\parallel }^2+\sum _l{\left(r_l\left(X\right)\right)}^T{\mathsf{\Lambda }}_l^{\mathrm{adj}}{r}_l\left(X\right)\right\}$$

(26)

By scaling ${\mathsf{\Lambda }}_l^{\mathrm{adj}}$, the proposed method reduces the influence of faulty sensors on the state estimation $X^{*}$ (which contains $p,v$) without resorting to loosely coupled linear blending. The final position and velocity are extracted from $X^{*}$ after solving the nonlinear least-squares problem.

In this way, the system can adaptively adjust the weights of different sensor types according to the dynamic changes in the residuals of the sensor observations and ensure that the effects of outliers is minimized while improving the robustness of the system. The specific optimization model is as follows: As shown in the Figure 3, a dynamic weighting factor is introduced to directly include or down-weight the sensor observations. This reduces the effects of sensor measurement errors on the positioning accuracy in complex environments. The specific Algorithm 1 is as follows:

Algorithm 1 Adaptive weighting algorithm

Require:   Residuals $r_{\rho }^{\left(k\right)},r_{IMU}^{\left(k\right)},r_{vision}^{\left(k\right)}$ Ensure:   Optimized state $X^{*}$ (positions p, velocities v) 1: Normalize residuals and compute test statistic $S_l$ via (16) 2: Set ${\mathsf{\Lambda }}_l={\mathsf{\Lambda }}_l^0$;    ${\alpha }_l\leftarrow \left\{\begin{array}{cc}1,\hfill & S_l\le T_l\hfill \\ T_l/S_l,& S_l>T_l\end{array}\right.$;   ${\mathsf{\Lambda }}_l\leftarrow {\alpha }_l{\mathsf{\Lambda }}_l$ 3: Apply M-estimator weights $w_{l,i}=\phi (r_{l,i}/{\sigma }_l)$ to all residuals 4: Build factor graph with updated ${\mathsf{\Lambda }}_l,w_{l,i}$ and solve (1) via Ceres to obtain $X^{*}$ 5: Extract $p,v$ from $X^{*}$ and return

4. Real-World Dataset Evaluation and Analysis

In this section, the proposed adaptive weighted factor graph-optimized positioning algorithm based on joint GNSS/IMU/vision residual detection is comprehensively validated in different scenarios, including complex open-sky and indoor–outdoor environments, through offline experimental validation with public datasets. All evaluation metrics, including Root Mean Square Error (RMSE in meters) and Absolute Trajectory Error (ATE in meters), are clearly defined with their units and calculated over the entire trajectory unless otherwise specified.

4.1. Open Sky

This test was validated using sports field sequences from the GVINS dataset [1], which were collected in an open-air sports field using a helmet with a VI sensor and a u-blox ZED-F9 P. The GNSS RTK solution was used as the ground-truth solution. According to Cao et al, a fixed solution of the GNSS RTK was used as the ground truth. As no reference pose was provided, this experiment focused on evaluating the localization performance. The proposed system was compared with several FGO-based GNSS visual inertial fusion methods, including GVINS and VINS-Mono [26]. The GVINS system itself is a tightly coupled FGO-based fusion algorithm that also uses a sliding window for optimization.

Figure 4 illustrates the GNSS observation conditions within the experimental environment in which the observability of the system was assessed using the PDOP. Throughout the experiment, the majority of satellites could be stably tracked, with PDOP values consistently remaining at approximately two. This indicates favorable observation conditions and an optimal configuration of the satellite geometry, thus supporting the reliability and accuracy of GNSS-based navigation methods.

Figure 5 shows the positioning errors associated with the three positioning methods. As depicted in the figure, VINS-Mono exhibits significant positional deviations in all directions, which are primarily due to the accumulation of sensor errors. Similarly, GVINS exhibits a notable deviation in the easterly direction, which is attributed to the uncorrected GNSS error terms. In contrast, the proposed algorithm effectively solves these problems by integrating error detection and weight-adjustment techniques. Consequently, it achieves more consistent and accurate positioning results in all directions.

4.2. Complex Indoor–Outdoor Environments

For the validation test, complex indoor and outdoor environment sequences from the GVINS dataset were used to evaluate the robustness of the proposed system. The experimental trajectories encompass various challenging scenarios that single-sensor localization systems usually cannot cope with. For example, extreme illumination changes can impair the effective detection of feature points, whereas indoor environments or in areas with strong signal interference, the GNSS becomes almost unusable. In addition, the trajectory resembles common exploration tasks in that it does not have a distinct closed-loop structure, which further complicates the ability of visual–inertial systems to fully mitigate drift-related problems.

Figure 6 illustrates the final localization trajectory within a complex environment, and the corresponding 3D localization trajectory is presented in Figure 7. It can be seen that the trajectory generated by the proposed algorithm (shown in red) closely aligns with the RTK ground truth trajectory (shown in yellow) for most of the path, highlighting its exceptional spatial localization accuracy. However, in the indoor staircase area, as magnified in the figure, the RTK trajectory exhibits a significant deviation due to GNSS signal blockage. In addition, the GVINS trajectory shows a noticeable drift towards the end of the localization path, which was caused by error accumulation. In contrast, the proposed algorithm effectively tracks the real path in 3D space by adaptively weighting the fusion of the GNSS, IMU, and vision sensor data. These results emphasize the capability of the algorithm for accurate 3D trajectory tracking and its superior performance in high-precision navigation tasks.

The following section provides a detailed analysis of the localization performance of different algorithms to further examine the error characteristics of the proposed algorithm and their effects. Special attention is given to complex environments, where observing error variations in different directions provides valuable insights into the anti-interference capabilities and overall robustness of the algorithms.

From the positioning error curves in the East, North, and Up directions shown in Figure 8, it can be observed that throughout the entire evaluation period, the error of the proposed algorithm (red curve) is significantly lower than that of GVINS (blue curve) and VINS-Mono (green curve). It also demonstrates superior error suppression capability compared to the factor graph optimization method that uses only robust M-estimation, M-FGO (cyan curve). Particularly during the strong interference interval from 1200 s to 1400 s, the positioning error of GVINS increases sharply. While M-FGO manages to partially suppress error growth, it still exhibits noticeable fluctuations. In contrast, by incorporating a robust M-estimation mechanism into the factor graph optimization framework to dynamically adjust observation weights, the proposed algorithm effectively curbs the spread of errors and maintains a more stable error level across all three directions.

Table 1. Comparison of RMSE and yaw errors.

	Proposed	GVINS	VINS-Mono	M-FGO
RMSE (m)	2.505	3.700	6.905	4.713
Yaw error (deg)	2.010	2.684	4.935	2.985

summarizes the RMSE and yaw error comparisons among the algorithms within the experimental environment. Overall, the proposed algorithm achieves superior error control, maintaining stable positioning accuracy over varying time periods and demonstrating enhanced robustness and precision.

Figure 9 illustrates the Absolute Trajectory Error curves for different algorithms over the traveled distance. Throughout the entire trajectory of 0–3000 m, the proposed algorithm (red curve) consistently maintains the lowest ATE and demonstrates the most stable error profile, reflecting its superior overall positioning accuracy and stability. Notably, around the 2000 m mark, where GNSS signals are likely subjected to interference, the GVINS method (blue curve) exhibits a significant error spike. In contrast, the proposed algorithm effectively suppresses the error surge through its robust fusion mechanism, maintaining smooth and low-error output. Meanwhile, although the M-FGO (cyan curve) and VINS-Mono (green curve) do not show drastic fluctuations, their overall error levels remain higher than that of the proposed algorithm. These results further validate the comprehensive advantage of the proposed algorithm in sustaining high precision and robustness over long-distance and complex environmental conditions.

To comprehensively evaluate the performance of the algorithms in dynamic environments, their accuracy and stability in velocity estimation are analyzed. Figure 10 illustrates the velocity errors of the proposed algorithm and GVINS in the X, Y, and Z directions. The key contribution of this figure is validating the effectiveness of the proposed adaptive weighting mechanism in suppressing velocity error accumulation. Specifically, the proposed algorithm maintains its velocity error within 0.25 m/s for most of the evaluation period. In contrast, GVINS exhibits significant fluctuations at approximately 1200 s due to GNSS signal interference, with peak errors reaching nearly 0.9 m/s. These results demonstrate that by dynamically adjusting sensor weights, the proposed algorithm effectively mitigates velocity error accumulation in dynamic environments, significantly enhancing the stability and anti-interference capability of velocity estimation.

In contrast, the proposed algorithm consistently maintains a low error level throughout the evaluation period. This performance advantage is attributed to the adaptive weighting mechanism integrated into the proposed algorithm, which is particularly effective in velocity estimation. By dynamically adjusting the sensor data weights in response to environmental changes or sensor fluctuations, the algorithm effectively suppresses the accumulation of velocity errors. As a result, the velocity estimation achieved by the proposed algorithm is not only smoother but also more accurate, demonstrating its robustness and adaptability in dynamic and challenging real-world environments.

5. Conclusions

In this study, an adaptive weighted factor graph optimization algorithm that incorporates joint residual detection was presented. The algorithm accumulates residual information from the GNSS pseudo-range, IMU pre-integration, and visual sensors within a sliding window framework and uses a combination of global weighting attenuation factors and the M-estimation method. This approach allows dynamic adjustment of the weights assigned to the different sensor data types in real time, effectively mitigating the effects of outliers on the state estimation. Offline experimental validation with real-world datasets shows that the proposed algorithm achieves superior positioning accuracy and robustness in both open-sky conditions and complex indoor–outdoor environments. In particular, the algorithm significantly reduces error accumulation and maintains a smooth 3D trajectory and velocity estimation, even under conditions of GNSS signal interference or blockage. This demonstrates a strong anti-interference capability, adaptability and versatility, making it well-suited for applications in autonomous driving and urban IoT. Future work will focus on dynamic modeling and compensation of sensor errors to further improve the positioning accuracy and adaptability of the system to diverse environmental conditions.