Factor graph optimization for GNSS/INS integration: A comparison with the extended Kalman filter

Factor graph optimization (FGO) recently has attracted attention as an alternative to the extended Kalman filter (EKF) for GNSS-INS integration. This study evaluates both loosely and tightly coupled integrations of GNSS code pseudor-ange and INS measurements for real-time positioning, using both conventional EKF and FGO with a dataset collected in an urban canyon in Hong Kong. The FGO strength is analyzed by degenerating the FGO-based estimator into an “EKF-like estimator.” In addition, the effects of window size on FGO performance are evaluated by considering both the GNSS pseudorange error models and environmental conditions. We conclude that the conventional FGO outperforms the EKF because of the following two factors: (1) FGO uses multiple iterations during the estimation to achieve a robust estimation; and (2) FGO better explores the time correlation between the measurements and states, based on a batch of historical data, when the measurements do not follow the Gaussian noise assumption.


INTRODUCTION
The Global Navigation Satellite System (GNSS) provides all-weather and globally referenced positioning in outdoor environments.However, the accuracy of GNSS positioning may be severely degraded in urban canyons with tall buildings, due to the multipath effect and non-line-of-sight (NLOS) (Groves, 2013) reception caused by reflections from and blockage by buildings.In contrast, an inertial navigation system (INS) (Zhuang & El-Sheimy, 2015;Zhuang et al., 2016) is less dependent on environmental conditions and provides relatively linear acceleration and angular velocity measurements at a high output frequency.However, INS suffers from error accumulation over time.Therefore, the GNSS and INS are complementary, and their integration to form GNSS-INS is a promising solution for vehicular positioning.
Various GNSS-INS integration frameworks were reviewed in Angrisano (2010).Thus far, the most integration solutions available are loosely coupled (LC) (Deng et al., 2017;Hsu et al., 2015;Solimeno, 2007), tightly coupled (TC) (Petovello, 2003), and ultra-tightly coupled (UTC) (Gao & Lachapelle, 2008) integrations.UTC integration requires the baseband signal processing of the GNSS receiver to be changed, which cannot be done by most GNSS-INS integrated system developers.The key difference between LC and TC integrations is in the measurement domain that is used.In LC GNSS-INS integration, the position and velocity estimated by the GNSS receiver are directly incorporated into the INS navigation solution.In contrast, TC integration uses raw GNSS measurements, such as pseudorange, Doppler frequency, and carrier phase measurements.TC GNSS-INS integration based on the extended Kalman filter (EKF) performs better than LC integration, as shown in Petovello (2003), largely because the estimator is optimized over a wider range of possibilities.In summary, TC GNSS-INS integration using the EKF is a common solution in current applications.
The recently proposed factor graph optimization (FGO) (Dellaert & Kaess, 2017) method is akin to a well-known approach in the robotics field for the development of visual (Qin et al., 2018) or light detection and ranging (LiDAR) simultaneous localization and mapping (SLAM) (Wen et al., 2018) to integrate diverse sensor measurements through nonlinear optimization.FGO also exhibits huge potential for GNSS-INS integration and therefore has attracted considerable attention (Li et al., 2018;Wen, Bai et al., 2019;Zhao et al., 2014Zhao et al., , 2016)).However, no previous study has used urban datasets to compare the performances of FGO and EKF algorithms for the development of low-cost LC and TC GNSS-INS integrations.The present paper aims to fill this gap, by showing how FGO can help mitigate the effects of GNSS outlier measurements on GNSS-INS integration and comparing the results with those obtained with the EKF estimator.In addition, we analyze the effects of window size on FGO performance and discuss the computational efficiencies of EKF and FGO.
The remainder of this paper is structured as follows: The studies related to the application of the EKF and FGO for GNSS-INS integration are reviewed in Section 2. The methodologies used to evaluate the four GNSS-INS integrations are presented in Section 3, and their experimental evaluations are presented in Section 4. Finally, conclusions and future work are presented in Section 5.

Role of EKF in GNSS-INS integration
The Bayesian filter (Thrun, 2000) has dominated GNSS-INS integration since the early 2000s.The Kalman filter (Welch & Bishop, 1995), the EKF (Julier & Uhlmann, 1997;Roysdon & Farrell, 2017), and the unscented Kalman filter (Wan & Van Der Merwe, 2000) are extremely popular, due to their maturity and computational efficiency in implementations.Several studies (Crassidis et al., 2007;Groves, 2015;Liu et al., 2010) integrated GNSS-INS sensors using an EKF estimator.Numerous practical applications demonstrated the robustness and effectiveness of the EKF in GNSS when the measurement quality is reasonable and the error noise is appropriately modeled.For example, EKF-based GNSS-INS integration is effective in both sparse and open areas with good sky visibility.
The conventional EKF efficiently achieves the optimal estimation if: 1. the first-order Markov chain is used, and 2. the random noise is modeled as Gaussian distribution (Barfoot, 2017).However, this is seldom the case when a GNSS receiver is located in urban areas.According to the findings obtained in (Zhao et al., 2016) and (Wen, Kan et al., 2019), GNSS measurements are non-Gaussiandistributed and highly time-correlated in urban canyons.Therefore, EKF-based sensor fusion fails to achieve optimal performance in such areas.From a mathematical perspective, under the Markov assumption, the EKF only evaluates the Jacobians at a single time-step (i.e., performs a single iteration) to achieve its recursive form.Thus, if an outlier is misjudged as a healthy measurement and the corresponding uncertainty is not modeled appropriately, the EKF is highly likely to be misled.This is unacceptable in applications that require accurate positioning services, such as unmanned aerial vehicles (Saripalli et al., 2003) and autonomous driving vehicles (Litman, 2015).In contrast, outlier measurement is frequently performed for GNSS positioning in urban canyons.To improve the performance of outlier measurement, the previous epochs of the state in the state vector may be considered.However, this substantially increases the size and decreases the convergence of the EKF estimator (Valiente et al., 2014).
Recently, the multistate constrained Kalman filter (MSCKF) (Li & Mourikis, 2013) was proposed for use in a visual SLAM field to integrate the information obtained from an INS and a camera.The MSCKF updates the states based on the geometry constraints of the feature measurements conducted inside a sliding window.However, to reduce the size of the features, their states are eliminated from the MSCKF by using a nullspace matrix.In addition, the EKF performance (Julier & Uhlmann, 1997) relies on accurate linearization, due to the nonlinearity of the observation function, and only a single linearization is performed in the EKF.Thus, the linearization accuracy relies heavily on the initial guess of the state.To address this issue, an iterated Kalman filter (Bell & Cathey, 1993) was used to perform multiple iterations during the update step, via the Gauss-Newton method, which helped to mitigate the error generated from the linearization steps.

FGO in GNSS-INS integration
The recently proposed FGO formulation (Indelman et al., 2013) has opened a new avenue for multisensor integration (Chen & Gao, 2019;Pfeifer & Protzel, 2019a, 2019b).It is represented as a probabilistic graphical model containing various nodes associated with the system states and factors representing the measurements.The factor graph encodes the posterior probability of the states over time, and unlike the conventional EKF-based integration, considers both historical measurements and system updates to optimize the complete state set.In this case, the historical information is used in FGO, and after all measurements and states are encoded into a factor graph, the sensor fusion problem is iteratively solved through optimization via the Gauss-Newton method.Therefore, the error resulting from the linearization steps is mitigated.Moreover, FGO handles delayed measurements, as these are simply additional sources of factors that are added to the factor graph as they are received.Thus, FGO is used in various challenging GNSS scenarios (Bhamidipati, 2018;Huang, 2016;Watson & Gross, 2018;Zheng Gong, 2018), and the authors in (Pfeifer & Protzel, 2018) showed the strong potential of FGO in sensor fusion, even when the sensor noise is modeled with a non-Gaussian distribution.
Researchers at the Georgia Institute of Technology (Indelman et al., 2012) used simulated data to demonstrate that the use of FGO in LC GNSS-INS integration led to better performance than the use of the EKF estimator.However, there was only a limited improvement in LC integration.A team from Tsinghua University (Li et al., 2018) extended this line of research by studying TC GNSS-INS integration and obtained substantially better performance than that achieved with the EKF estimator.Another study analyzed the performance of TC GNSS-INS-fisheye camera integration (Wen, Bai et al., 2019) using FGO based on a challenging dataset collected in urban canyons.In this study, a fisheye camera was innovatively used to model the uncertainty of GNSS pseudorange measurements before its integration with INS using FGO.The results indicated that TC FGO performed better than the TC EKF estimator.However, the LC integration performance of the EKF and FGO was not evaluated (Wen, Bai et al., 2019).
In another approach, researchers at the University of California, Riverside (Zhao et al., 2016), proposed an optimization-based sliding window for the integration of differential GNSS (DGNSS) and an INS, which they named the contemplative real-time (CRT) method.Based on sensor measurements, a Gauss-Newton method was then applied to optimize the states inside the window.CRT shares the same batch-processing theoretical basis with the factor graph approach.Moreover, its window is similar in size to the fixed-lag size considered in Indelman et al. (2013).The authors in Zhao et al. (2014) presented a CRT method for DGNSS-INS integration with a window size of 10 s and demonstrated the advantage of CRT over the TC EKF estimator.The error sources for the pseudorange measurements were mitigated through double-difference analysis before the CRT was integrated with the INS, leading to decimeter-level positioning accuracy.Therefore, the applicability of CRT in challenging urban canyons using a low-cost GNSS receiver, which may introduce a large pseudorange error, is yet to be explored.Continuous works are presented in Zhao et al. (2016) by adding more measurements into the CRT.However, CRT performance relies heavily on the size of the sliding window.Our published conference paper showed that different window sizes led to different performance improvements.The same phenomenon also was observed in our previous study (Wen, Kan, et al., 2019).As an extension to that study, the present paper makes two additional contributions, as below: • An extensive literature review is conducted, and a detailed analysis of existing sensor fusion schemes, including iterative EKF-, MSCKF-, and CRT-based fusion methods, is performed.

LC GNSS-INS integration using the EKF
The flowchart of the implemented EKF-based GNSS-INS integration is shown in Figure 1.The state-space of the  (Groves, 2013): where   is the transformation matrix used to transform the acceleration measurement from the local frame to the global frame based on   (see the Appendix for details).A generic dynamic model of EKF-based LC GNSS-INS integration can be written as where ( −1 ,   ) denotes the state transition function, and can be expressed as where Δ denotes the time difference between two epochs, and the function ( −1 ,   ) is based on the constantvelocity model.
Besides, the measurement model of EKF-based LC GNSS-INS integration can be expressed as where  ,  are the position measurements in the ECEF frame estimated by the GNSS receiver, and can be expressed as Note that  ,  is calculated using the weighted least squares method (Groves, 2013), based on the pseudorange code measurements obtained from the GNSS receiver.The observation function ℎ , (*) shows the relationship between the observation and the state at the k th epoch: where    is the Gaussian noise associated with the measurements, which is described with a covariance matrix equaling   .To calculate   , we follow the method used in Maier and Kleiner (2010), as follows: where   represents the user-equivalent range error (Maier & Kleiner, 2010), which we set as 10 m; ℎ  denotes the position dilution of precision (Groves, 2013), which is calculated based on the GNSS measurements; and  is a 3 × 3 identity matrix.

TC GNSS-INS integration using the EKF
The main difference between LC and TC integrations lies in the domain of the GNSS observations that are applied.TC integration uses raw GNSS pseudorange, while LC uses position measurements.The state-space of the system (  ) is represented as The state is similar to that used in LC integration, except that in TC integration the receiver clock bias   , must also be estimated.The measurements obtained from the INS and dynamic models are identical to those obtained in the LC integration.The measurement model of EKF-based TC GNSS-INS integration can be expressed as where  ,  indicates the GNSS pseudorange measurements (N satellites in total) performed in the ECEF frame using the GNSS receiver, and can be represented as The variable ℎ , (*) is an observation function that depicts the relationship between the observation and the state at the k-th time instant, The ionospheric and tropospheric errors are modeled based on (Herrera et al., 2016), and removed from the pseudorange measurements before its integration with the INS.Therefore, the observation function ℎ , (*) is formulated as follows: To determine the covariance matrix of   corresponding to the measurement vector  ,  , we follow the method proposed in Herrera et al. (2016).Here, each pseudorange measurement is expressed with a different uncertainty, based on its signal-to-noise ratio (SNR) and satellite elevation angle.Given a satellite with the SNR and elevation angle indicated as   and   , respectively, its weighting can be calculated as follows (Herrera et al., 2016): where T indicates the SNR threshold, and the parameters a, A, and F are selected based on (Herrera et al., 2016).Therefore, the covariance matrix   is a diagonal matrix constituted by the weighting   2 : with   2 = 1∕  (  ,   ).
The subscript N indicates the number of satellites and the matrix   is an  ×  matrix.

LC GNSS-INS integration using FGO
In general, multisensor integration aims to determine the optimal posterior state, based on sensor measurements.Therefore, the sensor integration problem can be formulated as a typical MAP problem (Barfoot, 2017).In this study, the measurements are performed in two parts, namely GNSS measurements and INS measurements.
Assuming that these measurements are independent of each other, GNSS-INS integration can be formulated as the following MAP problem: where  , represents the GNSS raw measurements performed at epoch k,   represents the system state at k, i denotes the index of the measurements performed at k (e.g., one epoch may have multiple pseudorange measurements),   denotes the control input (e.g., INS measurements), and x is the optimal system state set (Barfoot, 2017).The Bayes filter-based sensor fusion method recursively estimates the current epoch based on 1. the previous state, and 2. the control input and observation measurements at the current epoch.However, it fails to take full advantage of the historical information.Consequently, FGO-based sensor integration (Indelman et al., 2012) is applied to transform the MAP problem into a nonlinear optimization problem.
In FGO-based integration, all sensor measurements are treated as factors (  ) associated with specific states (  ).According to (Indelman et al., 2012), the MAP problem can be expressed as with   (  ) ∝ exp where   (  ) is a factor associated with a given measurement   , which can be derived from both GNSS and INS measurements;   is the state associated with the given measurements   ; ℎ  ( * ) is the observation function associated with   ; and the set  = { 1 ,  2 ,  3 , … ,   , …} denotes the states that must be estimated.Assuming that all of the sensor noise fits a Gaussian distribution, the negative logarithm of   (  ) is proportional to the error function (Indelman et al., 2012) associated with the measurements.Therefore, Equation ( 18) can be transformed as follows: Thus, FGO transforms Equation ( 17) into a standard nonlinear least-squares problem, expressed as Equation ( 19), and obtains the optimal state set  by minimizing the derived error function.
Figure 2 shows the graph structure of LC GNSS-INS integration using FGO, and the state space of the system is represented as Equation ( 1).The graph includes all historical observation measurements and shows a major difference between the conventional Kalman filter-based (Wan et al., 2018) and factor graph-based sensor integrations.The error functions of each listed factor are presented as follows.

Motion model factor
We use a constant-velocity model to constrain the two consecutive states.Based on this model, the motion model can be expressed as where   denotes the state at the given epoch  and ℎ  ( * ) represents the motion model function, and can be expressed as follows: The variable    is a covariance matrix associated with the motion model.This covariance matrix is constant and is formulated based on the specifications of the applied INS in this paper as follows: The units for the covariance matrix parts of

INS factor
In LC FGO, an INS provides linear accelerations that directly correlate with velocities between two epochs.The acceleration measurements performed in the global frame are denoted as (  ), based on Equation (3).The measurement model for linear acceleration is expressed as where the measurement function ℎ  ( −1 ,   ) is expressed as Here, the covariance matrix for the INS factor is expressed as   , .Therefore, the error function for INS acceleration measurements can be formulated as where    is a constant based on the INS specifications applied in this paper, as is defined as The unit for the covariance matrix is ∕.

GNSS factor
The error function for a given GNSS measurement for LC FGO is obtained as follows:

TC GNSS-INS integration using FGO
Figure 2 shows the graph structure of TC GNSS-INS integration using FGO, and the state space of the system is represented as Equation ( 10).The motion and INS factors remain the same as those used for LC FGO.

GNSS pseudorange factor
The GNSS receiver receives signals from multiple satellites at a given epoch k, which is expressed as Therefore, we obtain the error function for a given satellite measurement  , as follows: where   , denotes the covariance matrix, which is calculated using Equation ( 16).

LC GNSS-INS integration using FGO
We formulate three types of factors for use in LC GNSS-INS integration with FGO: motion model, INS, and GNSS factors.Therefore, the optimal state set  = { 1 ,  2 ,  3 , … ,   , …} is solved as follows: We also formulate three types of factors for use in TC GNSS-INS integration with FGO: motion model, INS, and pseudorange factors.Therefore, the optimal state set  = { 1 ,  2 ,  3 , … ,   , …} is solved as follows: (32) During the optimization, the Levenberg-Marquardt method is used to solve Equations ( 31) and (32).

setup
evaluate the performance of the four abovementioned integration schemes, we conducted experiments in an urban canyon in Hong Kong.The experimental vehicle and test scene are shown in Figure 3.The image on the left of Figure 3 shows the experimental vehicle, in which all sensors were installed in a compact sensor kit.The image on the right of Figure 3 shows the tested urban canyon in Hong Kong, which contained tall buildings, a challenging scenario for GNSS positioning.
During the test, a low-cost u-blox M8T GNSS receiver was used to collect raw single-frequency global positioning system (GPS) and BeiDou measurement data at a frequency of 1 Hz.The Xsens Ti-10 IMU was used to collect data at a frequency of 100 Hz, and a fisheye camera was used to capture a sky-view image to show the environmental conditions, for reference only.In addition, the NovAtel SPAN-CPT, which is a GNSS real-time kinematic-INS (fiber-optic gyroscope) integrated navigation system, was used to obtain the ground truth.The gyro-bias in-run stability of the FGO was 1 • /h and its random walk was 0.067 • /h.The baseline between the vehicle and the GNSS base-station was approximately 7 km.Besides, a positioning accuracy of approximately 10 cm was obtained using SPAN-CPT in the evaluated scene.All data were collected and synchronized using a robotic operating system (Quigley et al., 2009).Before conducting the experiments, the coordinate systems between all sensors were calibrated.We ran the EKF and FGO on a high-performance desktop computer equipped with an Intel i7-9700K at 4.20 GHz and 64GB RAM, in a postprocessing manner.The INS specifications that were applied are listed in Table 1.
Because the choice of covariance parameters is known to substantially affect GNSS-INS accuracy, we used the same parameters for the EKF and FGO based on Equations ( 9) and ( 16), respectively.We focused on analyzing the differences between the EKF and FGO.The estimated state was in the ECEF (Groves, 2013).We transformed the positioning results obtained from ECEF into an ENU frame (Groves, 2013).As the orientation was directly obtained from the AHRS, only the two-dimensional (2D) positioning (north and east directions) accuracy was evaluated.

Comparison of positioning accuracy
The positioning performances of the integrations conducted in the tested urban canyon are shown in Table 2, where 9.14 m of 2D mean error was obtained using LC EKF with a standard deviation of 7.60 m.The mean error decreased to 8.03 m when the TC EKF method was used.Moreover, the time efficiencies of LC and TC EKF were found to be similar, with less than 0.1 s required to process all data.After applying LC FGO for GNSS-INS integration, the 2D error decreased to 7.01 m with a standard deviation of 6.41 m, which was better than the value obtained for TC EKF.Moreover, the standard deviation decreased from 7.15 to 6.41 m.However, 15.41 s were required to process all data.Notably, the 2D mean error decreased to 3.64 m when TC FGO was used.The standard deviation also decreased dramatically, from 6.41 to 2.84 m, compared to when LC FGO was used.However, the computational time increased substantially (to 75.30s), due to the increased number of factors in TC FGO compared to LC FGO.In summary, the optimal performance was obtained using TC FGO.As all historical data were considered in FGO, this led to a greater computational load.The trajectories and positioning errors of the two LC and two TC integrations generated during the test are shown in Figures 4 and 5, respectively.The right side of Figure 4 shows that the mean error of LC FGO is slightly less in most epochs compared to that of LC EKF.However, the mean error is substantially less when TC FGO is used, as shown on the right side of Figure 5.
The residual term evaluates the difference between the measurements and final state estimation.If all applied measurements and their associated error model are correct, the residual should be zero.However, the residuals are usually nonzero, due to the noise generated by signal blockage or reflection, leading to NLOS reception in the GNSS pseudorange measurements.Both the EKF-and FGO-based methods aim to minimize the residuals of all considered measurements based on the associated covariance matrix.Smaller residuals usually indicate that the estimated state is closer to its optimal estimation.Thus, the value of a residual is a good indicator of the quality of GNSS-INS integration.Therefore, we also present the residual results of the four integrations used in this study.
The residuals of the LC and TC integrations are shown in Figures 6 and 7, respectively.In LC integration, the observation is the measurement of GNSS positioning solutions.We calculate the L2 norm of the residual ( , ), which denotes the difference between the GNSS positioning measurements and the final GNSS-INS integration result, as follows: where  , denotes the GNSS residual in LC integration and the operator ()  is used to obtain the 2D part (horizontal positioning).Thus, we evaluate 2D residuals for LC integration.Therefore, the 2D positioning error (right side of Figure 4) and residual (Figure 6) are the same metrics and may be compared, where larger residuals usually indicate a larger potential positioning error.throughout the test, the FGO-based method has a similar or smaller residual than the EKF-based integration.Figure 7 shows the residual of the pseudorange measurements.To simplify the comparison of the pseudorange residual and mean positioning error of GNSS-INS integration, we calculate the mean of the L1 norm of the pseudorange residual in TC integration, as follows: where  , denotes the GNSS residual in TC integration at a given epoch k.As shown in the figure, the FGObased method yields a much smaller residual.Note that the residual for TC integration lies in the GNSS pseudorange domain.A small residual indicates that the final positioning result is closer to the given measurement.The figure also shows that the TC FGO residual is substantially smoother than the TC EKF residual.In sum, FGObased GNSS-INS integration exhibits better accuracy than EKF-based integration.Moreover, FGO-based TC integration exhibits the best performance of the four listed integrations.

Analysis of improvement in FGO vs. window size
According to (Dellaert & Kaess, 2017), a major difference between the EKF and FGO approaches is the number of "iterations" applied.The EKF estimator only iterates once based on the given observation measurements, whereas FGO involves several iterations, which consider all historical and current measurements simultaneously, to approach the optimal state.If the window size of the optimization is set to 1 s [K = 1 in (32)], which means that FGO-based TC integration only considers the measurements at the current and last epochs, the major difference between the FGO and EKF approaches lies in the number of iterations applied during GNSS-INS integration.The window size denotes the epoch of historical states considered in FGO.We refer to the FGO with a window size of 1 s as the "EKF-like estimator."Note that this estimator differs from the EKF, which recursively maintains the historical information present in the covariance matrix.Figure 8 shows the 2D positioning error of TC GNSS-INS integration obtained using FGO under different window sizes.The 2D mean positioning error is 5.18 m (black curve in Figure 8) with a window size of 1 s, which is better than that obtained from EKF-based TC integration (8.03 m).This reduction in the mean positioning error is largely due to the number of iterations used in the "EKF-like estimator" compared to the number used in the EKF, which is a key difference between these two approaches.The other key difference between the EKF and FGO approaches is that more historical data are considered simultaneously during FGO.To analyze the effects of window size on the performance of FGO-based GNSS-INS integration, the positioning results obtained using different window sizes are also shown in Figure 8.Note that a window size of 256 s equals the batch optimization, which considers all historical information in FGO.With a window size of 1 s, there is a limited improvement in the FGO.With the increased window sizes (5, 10, 30, and 256 s), more historical information is considered during the optimization, and the overall FGO performance gradually improves.Besides, the error curve (blue) arising from batch optimization (window size of 256 s) is substantially smoother than the other curves.Overall, it can be seen that the use of more historical data tends to increase resilience to outliers in GNSS measurements, such as NLOS receptions and multipath effects.We find that when the window size is approximately 30 s, the accuracy (3.74 m) is close to that (3.65 m) obtained for batch optimization (window size is 256 s).
The improvements obtained from FGO-based GNSS-INS integration can be summarized as follows.
1. Multiple iterations: As FGO is a process of determining the optimal estimation based on the gradient (Dellaert & Kaess, 2017), multiple iterations help to estimate the optimal state.Thus, these iterations relax the requirement for the accuracy of the initial guess of the estimators.In addition, linearization is conducted The line-of-sight (LOS) connecting the GNSS receiver and satellite is accurate, even when the initial guess has an error of 100 m.This has also been mentioned in (Zhao et al, 2014).2. Time correlation: FGO effectively considers historical information, all of which are connected by the INS factor.Thus, the time correlation between the historical epochs is simultaneously explored and used to resist the outliers.Thus, an appropriate FGO window size improves the performance.The same finding has also been discussed in (Zhao et al., 2016).

Case study on the relationship between the measurement uncertainty and the selection of window size for FGO
As shown in Figure 8, the error (blue curve) of batch optimization at epoch D is larger than those resulting from smaller window sizes (e.g., 20 s, 10 s, and 5 s).This means that in some epochs a larger window size does not nec-essarily lead to better FGO performance.Figure 9 shows the sky-view images captured by a fisheye camera and the satellite visibilities for the four selected epochs (A, B, C, and D) in Figure 8.The blue and red circles denote the LOS and NLOS satellites, respectively, which are classified according to our previous study (Wen, Bai, et al., 2019) If the error model and relative weightings [corresponding to Equations ( 9), (15), and ( 16)] of the pseudorange measurements are provided, the use of more historical data should improve the accuracy of GNSS-INS integration.However, if the error modeling is incorrect, the time correlation will not be accurate, and a larger window size will lead to only limited improvements or even worse accuracy (epoch D in Figure 8).Thus, the limited improvements obtained under the increased window size (from 30 to 256 s) are partially caused by the imperfect weightings (modeling of noise covariance).Both EKF-and FGO-based GNSS-INS integrations rely on the assumption that the sensor noise can be fitted by a Gaussian model.However, this assumption is usually violated due to the satellite signal reflection and blockage caused by buildings.This is a major factor limiting the performance of GNSS-INS integration in urban canyons.Instead of using Gaussian distribution, a team from the Chemnitz University of Technology (Pfeifer & Protzel, 2019a) recently has proposed the use of GMM for modeling the pseudorange measurement noise.They found that if the residuals of all measurements performed inside a window effectively model the error distribution of the measurements at the current epoch, FGO can be substantially improved based on the GMM estimated using the historical pseudorange residuals.The authors (Pfeifer & Protzel, 2019a) argued that the noise of pseudorange measurements in urban canyons does not have a Gaussian distribution.Inspired by their work, we show the pseudorange error and residual distributions near epoch D under different window sizes.
Figure 10 shows the measured GPS/BeiDou pseudorange errors, which are labeled based on the ground-truth trajectory and three-dimensional building modeling using the double-difference technique (Xu et al., 2019).Thus, the figure shows the true error distribution of the pseudorange measurement.Note that the pseudorange errors are within a 30-s window of epoch D, as indicated by the red rectangle in Figure 8.According to our fitting analysis, three Gaussian components are sufficient to fit the histograms of Figure 10 with three major peaks.This agrees with the finding presented in Pfeifer & Protzel (2018), in which three Gaussian components were usually enough to model the pseudorange error distribution in urban canyons.The histogram exhibits a long tail on the right side, which is largely due to NLOS receptions caused by building reflections.This is one of the main reasons for the non-Gaussian property of pseudorange measurements.A similar long-tailed phenomenon is also witnessed in the BeiDou pseudorange errors on the right side of )) , (35) where the actual values (positive and negative) are used.Similar histograms and GMM parameters (see the table inside Figure 11) may be obtained using the pseudorange residuals.This means that the fitted GMM in Figure 11 comprising the pseudorange residuals inside a 30 s sliding window effectively represents the actual error distribution (the GMM shown in Figure 10).As Figure 8 shows, a window size of 30 s leads to similar accuracy to that obtained from the batch optimization.This means that the historical measurements performed within the 30-s win- In sum, the residuals obtained within the window size of 256 s at epoch D cannot effectively describe the actual noise at this epoch (Figure 10).Thus, this is a significant factor that defines the optimal window size in FGO.

4.5
Discussion on the computational load of FGO Another problem faced in FGO applications is the increased computational load compared to that generated by the use of the EKF estimator.To maintain real-time performance, a sliding window may be used, with only the measurements within this sliding window being considered for FGO.However, this technique does not use all of the historical data.To address this issue, the incremental smoothing and mapping (iSAM) algorithm (Kaess et al., 2008) has been proposed and is built on top of the factor graph.A key advantage of iSAM is that it guarantees a low computational load even when large amounts of data are used in the optimization.This is achieved by storing the connections between the historical data in a novel Bayes tree structure.This means that iSAM does not need to recalculate the Jacobians for the states that are unaffected by the newly performed measurements.Figure 13 shows a time comparison of the two TC GNSS-INS integrations solved by FGO and by iSAM.The computational time F I G U R E 1 3 Comparison of the computational time used in tightly coupled (TC) integration using factor graph optimization (FGO) and the TC incremental smoothing and mapping algorithm (iSAM) (Kaess et al., 2008) [Color figure can be viewed in the online issue, which is available at wileyonlinelibrary.com and www.ion.org]used by FGO increases dramatically when more historical data are used, with 75.30 s required to process all of the data.However, iSAM has a substantially lower computational load when incorporating more historical data, with only 15.41 s required to process all the data.Meanwhile, the slope of the curve shown in Figure 13 is almost constant during the GNSS-INS integration.Thus, although the amount of historical data increases, the time used to process each epoch remains almost constant.Moreover, the positioning performances of both methods are almost the same.Thus, we believe that iSAM is a suitable alternative for solving FGO-based sensor fusion in real-time.

CONCLUSIONS AND FUTURE WORK
This paper comprehensively compares the performance of four GNSS-INS integrations using both the EKF and FGO, using a real dataset collected in urban canyons.The experimental results show that TC GNSS-INS integration performs better than LC GNSS-INS integration.Besides, TC integration using FGO performs the best of the four integrations.According to the analysis results, the superior performance of FGO compared to that of EKF is attributable to the 1. multiple iterations, and 2. greater amount of data applied in the FGO.Therefore, we believe that FGO-based sensor fusion may be a promising replacefor EKF-based methods in the coming decades.Besides, an analysis of the effect of window size on FGO performance shows that appropriate window size is highly correlated with environmental conditions.Recently, context awareness (Gao & Groves, 2020) has been used to identify the potential sensor noise generated in GNSS positioning.The use of environmental context-awareness to identify the period during which sensor noise characteristics are similar or constant is a promising way to adaptively tune the window size.For example, by limiting the window size to align with the time over which the error models are roughly constant or similar, correct relative weightings are preserved, which likely contribute to the better performance obtained for adaptive window size.
As this paper analyzed only one dataset collected in a typical urban canyon of Hong Kong, it will be interesting and necessary to determine how FGO will handle multiple datasets from diverse urban scenarios.In the future, we will examine FGO performance using more sensors (e.g., LiDAR) with our recently published UrbanLoco dataset (Wen et al., 2020), which comprises a complete set of vehicular navigation sensor data collected in both Hong Kong and downtown San Francisco.where ∅  and ∅  represent the longitude and latitude, respectively, based on the WGS84 geodetic system (Groves, 2013), which can be derived from   .The variable   is the transformation matrix that transforms the acceleration measurements obtained from the body to the local frames based on   , , which can be expressed as follows:

F
I G U R E 3 The left image shows the experimental vehicle and sensor setup, and the right image illustrates the tested urban canyon in Hong Kong [Color figure can be viewed in the online issue, which is available at wileyonlinelibrary.com and www.ion.org]

F
Trajectories of TC GNSS/INS integrations using EKF and FGO in the east, north, and up (ENU) frame.The black curve denotes the reference trajectory.The red and blue curves in the left-hand side figure represent the trajectories from TC integrations using EKF and FGO, respectively.The right figure shows the 2D error [Color figure can be viewed in the online issue, which is available at wileyonlinelibrary.com and www.ion.org]F I G U R E 6 2D residuals of the loosely coupled GNSS/INS integrations using EKF and FGO [Color figure can be viewed in the online issue, which is available at wileyonlinelibrary.com and www.ion.org]

F
I G U R E 8 2D positioning errors under different window sizes used in TC GNSS/INS integrations using FGO.The x-axis denotes the epochs, and the y-axis represents the value of 2D positioning errors.The red and green rectangles denote the sliding windows of sizes 30 and 256 s, respectively [Color figure can be viewed in the online issue, which is available at wileyonlinelibrary.com and www.ion.org]

F
Sky-view and satellite visibilities of the four selected epochs in Figure 8.The blue and red circles denote the line-of-sight (LOS) and non-line-of-sight (NLOS) satellites, respectively [Color figure can be viewed in the online issue, which is available at wileyonlinelibrary.com and www.ion.org] in each FGO iteration, which helps to linearize the nonlinear observation model.However, the nonlinearity of the model is trivial for TC GNSS-INS integration: . The errors peak at epochs A, C, and D due to the severe NLOS receptions (see red circles).The period near epoch B introduces a similar positioning error (3-6 m) under different window sizes.The actual sensor noise (error magnitude) model near epoch B is substantially different from that near epoch D. Thus, the historical measurements fail to reflect the measurement noise at the current epoch D. A larger window size (see blue curve in Figure 8) in FGO leads to a worse result at epoch D.

F
Histogram of GPS (left)  and BeiDou (right) pseudorange errors and the corresponding fitted Gaussian mixture model (GMM) inside a sliding window of epoch D with a size of 30 s (as indicated by the red rectangle in Figure8).The x-axis denotes the value of pseudorange errors acquired by the ray-tracing technique.The y-axis denotes the counts of errors within the histogram.The blue curve represents the GMM fitted by the histogram using three Gaussian components [Color figure can be viewed in the online issue, which is available at wileyonlinelibrary.com and www.ion.org] Figure 10.We use a GMM with three Gaussian components, which is shown by blue curves in the figure, to quantitatively fit the GPS/BeiDou error distributions.The mean, standard deviation, and weighting of each Gaussian component are also shown in the figure.The first component of the GMM, for GPS pseudorange measurements with a mean of 38.76 m, represents the NLOS signals, which generate the long-tail phenomenon.The first component of GMM, for BeiDou pseudorange measurements with a mean of 32.62 m, also represents the NLOS signals.In sum, the numerous NLOS receptions lead to a pronounced long-tail phenomenon in the error distribution.Similar to Figure 10, Figure 11 also shows the residuals of GPS/BeiDou pseudorange measurements, which are calculated as follows:

F
Histogram and Gaussian mixture models (GMMs) of pseudorange residuals near epoch D with a sliding window of 30 s.These are similar to Figure 10, with the major difference being that the histogram and GMMs are based on the residuals [Color figure can be viewed in the online issue, which is available at wileyonlinelibrary.com and www.ion.org]I G U R E 1 2 Histogram and Gaussian mixture models (GMMs) of pseudorange residuals similar to Figure 11, with the major difference being that the histogram and GMMs are based on the residuals near epoch D with a sliding window of 256 s [Color figure can be viewed in the online issue, which is available at wileyonlinelibrary.com and www.ion.org]dow describe the error distribution of the measurement noise near epoch D. Figure 12 shows the distributions of the residuals with substantially larger window size (256 s) than that in Figure 10.The left figure shows that the GPS residuals introduce a long-tail phenomenon, with the maximum pseudorange residual reaching 100 m, which is substantially different from that observed in Figure 11.Thus, the Gaussian component with a mean of 36.58 m introduces a large standard deviation of 1,100.6 m.The mean values (10.07 m) of the GMM for the BeiDou pseudorange measurements are substantially less than the 32.62 m value shown on the right side of Figure 10.

F I G U R E 1
Flowchart of the loosely coupled (blue line) and tightly coupled (red line) GNSS-INS integrations implemented using the EKF [Color figure can be viewed in the online issue, which is available at wileyonlinelibrary.com and www.ion.org]system (  ) is represented as measurement at epoch k, N denotes the total number of satellites at epoch k, and    is the Gaussian noise associated with the measurements.The position of a satellite  , is represented as  , = ( ,represents the ith pseu-dorange Positioning performance and computational load (time) of the four methods Trajectories of the LC GNSS/INS integrations using EKF and FGO in the east, north, and up (ENU) frame.The black curve denotes the reference trajectory.The red and blue curves in the left-hand side figure represent the trajectories from LC integrations using EKF and FGO, respectively.The right figure shows the 2D error [Color figure can be viewed in the online issue, which is available at wileyonlinelibrary.com and www.ion.org] TA B L E 2