A Multi-Sensor Information Fusion Method Based on Factor Graph for Integrated Navigation System

,


I. INTRODUCTION
Autonomous mobile robotic systems have been applied in various fields, e.g., health, transportation, and military [1].Accurate and reliable navigation is essential in such systems and has become a topic of significant research interest [2].Compared with the early inertial navigation systems in which the inertial measurement unit (IMU) is the prime sensor, current systems are always equipped with additional sensors to get more accurate and reliable navigation solutions.Although the combination of different sensors can improve navigation accuracy, the asynchronism and nonlinearity of sensor measurements make it difficult to fuse multi-sensor information efficiently [3].
Since its proposal in 1960, Kalman Filter (KF) has been widely used to solve many difficult and complicated information fusion problems [4].However, the classical Kalman Filter can only be applied in linear systems.Although in The associate editor coordinating the review of this manuscript and approving it for publication was Salvatore Surdo .
the following decades, Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF) were proposed for nonlinear systems [5]- [10], with the increasing number of sensors in the navigation system, these centralized filters [11] can result in an enormous computational burden and poor fault tolerance [12].Recently, some work has been proposed to improve the performance of KF in the navigation system.Hailiang et al. presented a novel robust single GPS navigation algorithm based on dead reckoning and strong tracking filter [13].More recently, an unconventional multi-sensor integration strategy based on the kinematic trajectory model was proposed in [14].To solve the dilemma of navigation accuracy and computation cost, Speyer et al. proposed the idea of a decentralized filter and developed it for optimal estimation problems [15]- [21].In 1987, Federated Filter (FF) was presented and became a representative of a decentralized filter because of its flexibility, stability, and good fault tolerance capacity [22].Yuan et al. presented a federated extended finite impulse response filter to obtain accurate position information [23].However, in practical applications, an integrated navigation system always consists of multi-rate sensors.Thus, Federated Filter needs time registration before fusing the information [24], which complicates the fusion process.Even if the federated filter in [25] improved the time registration process, it still takes some time.
In a multi-sensor integrated navigation system, there are always some cases when some sensors are unavailable.It requires the navigation system to react quickly and recover the navigation accuracy in a short time.Hailiang et al. presented a novel robust fault-tolerant federated filter to improve the stability of the navigation system [26].Kai-wei et al. proposed new strategies for the integrated navigation system to achieve Simultaneous Localization and Mapping (SLAM), especially in GNSS challenging environments where GNSS signals are blocked [27].This work only considers the situation that the GNSS signal is blocked, and the fusion framework needs reconstruction when the sensor signal is lost.Wei et al. presented an integrated positioning methodology for train navigation application to solve the problem of positioning during BeiDou navigation satellite system (BDS) outages [28].This work utilizes a switch module to transfer between two navigation systems with and without BDS signal, which means the navigation system needs to be reconstructed once a specific sensor is unavailable.Although the federated filter is exploited for its flexibility and capability in fault toleration, it needs to reconstruct its framework when sensors are added or removed.Therefore, it is worthwhile to propose a multi-sensor information fusion method to achieve real-time adaptive information fusion.
A recent study in monocular visual-inertial navigation has shown that optimization-based approaches outperform filtering methods in terms of accuracy [29].As one of the optimization-based methods, the factor graph has recently been used in SLAM and navigation problems.Since 2006, Michael et al. have presented a series of Simultaneous Smoothing and Mapping (SAM) algorithms for robot SLAM problem [30]- [33].Sven et al. applied a factor graph to the Unmanned Aerial Vehicle (UAV) navigation system [34].Inspired by the idea of SAM algorithms, Han-Pang et al. proposed an integrated navigation system based on a factor graph for a robot, which includes 57 different sensors [35].More recently, David et al. employed the context of so-called ''Dynamic Networks'' to fuse the processing of the image, raw inertial, and GNSS observations for UAV applications [36].Qinghua et al. presented a factor graph-based navigation method for Micro Unmanned Aerial Vehicle (MUAV) [37].Xiao-kai et al. proposed an INS/Odometer integrated navigation algorithm based on factor graph for the vehicle integrated navigation system, but there are only two sensors in the system [38].Although it has been shown that factor graph-based methods have good performance in processing asynchronous data, the current work mostly focuses on small-scenario, low-dynamic SLAM problems and is almost verified through simulation.There is no real-world large-scenario experiment with more than three sensors so far.This paper proposes a multi-sensor information fusion approach based on a factor graph for integrated navigation systems.Specifically, we make the following contributions.
• We present a method that uses a factor graph for fast computation of the navigation state estimation.The first step towards this goal is constructing the factor graph framework for the multi-sensor navigation system.We analyze the different sensors' measurement models and formulate corresponding factors.Then we build the factor graph framework for the integrated navigation systems in the experiments.
• We perform two real-world experiments to verify the proposed method's effectiveness in small and large scenarios.One is based on the KITTI dataset, and the other one is based on a dataset collected by our laboratory.
• We investigate the sensor plug and play experiment to prove that the proposed method can react quickly in different environments.For example, when the GPS signal is poor or unavailable in some cases, the proposed method can continue fusing other available sensor information in a short time.The rest of the paper is organized as follows.In Section II, we present a brief introduction about the factor graph and Bayesian maximum a posterior.Section III introduces the multi-sensor information fusion method based on the factor graph.Experimental results are presented and analyzed in Section IV, followed by the conclusions summarized in Section V.

II. BACKGROUND
This section introduces the concepts of factor graph and Bayesian maximum a posteriori used to build the navigation system's factor graph framework.

A. FACTOR GRAPH
A factor graph is a bipartite graph representing the factorization of a function.Given a factorization of a function g(θ 1 , . . ., θ n ): where i ⊆ {θ 1 , . . ., θ n }, the corresponding factor graph G = ( , F, E) consists of variable nodes = {θ 1 , . . ., θ n }, factor nodes F = {f 1 , . . ., f m }, and edges E. The edges depend on the factorization i.e., there is an undirected edge between factor node f i and variable node θ j iff θ j ∈ i .For example, consider a function that can be factorized as: with a corresponding factor graph shown in Fig.

B. BAYESIAN MAXIMUM A POSTERIORI
In Bayesian statistics, a maximum a posteriori (MAP) probability estimate is an estimate of an unknown quantity that equals the mode of the posterior distribution [39].The MAP can be used to obtain a point estimate of an unobserved quantity based on empirical data.It is closely related to the method of maximum likelihood (ML) estimation.However, it employs an augmented optimization objective that incorporates a prior distribution (that quantifies the additional information available through prior knowledge of a related event) over the quantity we want to estimate.MAP estimation can, therefore, be seen as a regularization of the maximum likelihood estimation.
Let us assume that we want to estimate an unobserved population parameter θ based on observations x.Let f be the sampling distribution of x, so that f (x|θ) is the probability of x when the underlying population parameter is θ.Then the function θ → f (x|θ) is known as the likelihood function and the estimate θMLE Next, assume that a prior distribution g over θ exists.So the θ can be treated as a random variable in Bayesian statistics.The posterior distribution of θ can be calculated using Bayes' theorem: where g is density function of θ, is the domain of g.
The method of maximum a posteriori estimation then estimates θ as the mode of the posterior distribution of this random variable: where the denominator of the posterior distribution (so-called marginal likelihood) is always positive and does not depend on θ and therefore plays no role in the optimization.

III. INFORMATION FUSION METHOD BASED ON FACTOR GRAPH
Based on the factor graph and maximum a posteriori as presented in Section II, we now propose the multi-sensor information fusion method for an integrated navigation system using the factor graph.This section first defines the optimal navigation solution as a maximum a posteriori estimation of the navigation state.Second, we construct the factor graph framework for an integrated navigation system by analyzing measurement models in various navigation sensors and formulating corresponding sensor factors.Furthermore, we utilize the Gaussian-Newton iteration method to solve the nonlinear optimization problem.As a result, the proposed method can fuse asynchronous, nonlinear multi-sensor information efficiently.

A. MAXIMUM A POSTERIORI OF NAVIGATION STATE
Based on the idea of factor graph mentioned in Section II-A, we can construct a factor graph for integrated navigation system.In the graph, factor nodes and variable nodes stand for sensor measurements and navigation states, respectively.By calculating the maximum a posteriori estimation of joint probability distribution function for navigation states in a period of time, we can get the optimal estimate of navigation states with all available asynchronous multi-sensor data.The state variable set with the maximum a posteriori probability is considered as follows: where X 0:k is the state variable set from t 0 till t k , Z 1:k is the measurement set from all available sensors till t k , and X0:k is the maximum a posteriori estimation calculated by maximizing the right side of ( 5).According to the Bayes formula, ( 5) can be factorized as: And because the joint probability distribution function can be factorized in terms of a priori information and individual process and measurement models.Such a factorization can be finally written as: where p(X 0 ) is the prior information, p( are the probability distribution of navigation state and calibration parameter in the inertial navigation system, respectively.Finally, p(Z j i |X j 0:k,i ) is the probability distribution of other navigation sensors' measurements.For example, if This factorization process is similar to that in the factor graph -from a global function to the product of local functions.Therefore, it is reasonable and feasible to apply factor graph to multi-sensor information fusion in integrated navigation system.
In the factor graph, assume that f (.) represents the local probability distribution function, then (6) can be written as: where X 0:k,i is the subset of the state set X 0:k , local function f i is related to error function err(X 0:k,i , Z i ).So f i can be formulated as: where d(.) is the cost function.
Assuming a Gaussian noise model, ( 8) can be written as: where . 2 denotes the squared Mahalanobis distance, and i is the measurement noise covariance matrix at time t i .So, calculating the maximum a posteriori estimation in ( 5) In practical navigation system, the error function err(X 0:k,i , Z i ) can be represented as Z i − H i (X 0:k,i ), where H i (.) is the measurement model of the corresponding sensors to predict sensor measurements given a set of states.Then the error function captures the error between the predicted measurements H i (X 0:k,i ) and the actual measurements Z i received from sensors.

B. FACTOR GRAPH FRAMEWORK FOR INTEGRATED NAVIGATION SYSTEM
Based on the formulations given in Section III-A, we can construct the factor graph framework for different integrated navigation systems.In the framework, the factor nodes are local functions f (.) in (7) and the variable nodes are navigation states X 0 , X 1 , . . ., X k and inertial calibration parameters c 0 , c 1 , . . ., c k .Since different sensors have different measurement models, we need to present a distinct factor node formulation for each sensor.Considering the sensors used in our experiments, we analyze measurement models in IMU, Global Positioning System (GPS), BeiDou Navigation Satellite System (BDS), baro-altimeter, visual odometer, and odometer.

1) IMU FACTOR
For inertial measurement unit, the time evolution of the navigation state X can be described by the following continuous nonlinear differential equation: where f b and ω b are the specific force and the angular acceleration, respectively, measured by the inertial sensors in body frame.
The IMU calibration parameters represented by c are used to compensate for the bias error of IMU measurement f b , ω b according to the assumed IMU error model.It is usually estimated at the same time when the navigation state is estimated.Furthermore, the time evolution of c can be described as follows: A given IMU measurement Z IMU i = f b i , ω b i relates the navigation states at two consecutive time instances t i−1 and t i .The discrete representation of ( 12) and ( 13) is: where navigation state X i is predicted by current inertial measurement Z IMU i and previous state and calibration parameter Based on the representation of error function and ( 9), the conventional IMU factor f IMU and bias factor f bias can be given as: where we can find that the IMU factor is related with the current state, previous state and bias, while the bias factor is related with the current and previous bias.The corresponding factor graph with IMU and bias factors is shown in Fig. 2 (a).In general, estimating the state variables at IMU frequency that is very high is infeasible.Instead, consecutive IMU measurements can be combined into an equivalent IMU factor [29], [40], relating between two distant navigation and calibration nodes, which can reduce the cost of computational complexity dramatically by decreasing the number of variable nodes in factor graph, as shown in Fig. 2 (b).

2) GPS FACTOR
The GPS measurement equation is given by: where N GPS is the measurement noise and H GPS is the measurement function, relating the measurement Z GPS i to state X i .The above equation defines the GPS factor f GPS : VOLUME 9, 2021  which is only related with the current navigation state X i .Fig. 3 shows a factor graph of an INS/GPS integrated navigation system in which IMU and GPS measurements are added into the graph with different frequencies.Note, BDS factor and baro-altimeter factor equations are similar to the GPS factor equation.

3) VISUAL ODOMETER FACTOR
Visual odometer has become popular in current navigation systems [41] as it allows us to estimate the relative transformation between two stereo frames, assuming a known baseline.Based on this relative transformation information, we can get the current position of the target.The visual odometer measurement model can be given by: where Z VO ij is the measurement based on state set X i , X j and N VO is measurement noise.The corresponding visual odometer factor is: Fig. 4 is an example of a factor graph in IMU/Visual odometer integrated navigation system.Odometer factor equation is similar to visual odometer factor equation.

C. COST FUNCTION LINEARIZATION
As many sensor measurements in integrated navigation system appear to have a nonlinear character, ( 10) is a nonlinear least square problem.We use the Gaussian-Newton iteration method to solve this optimization problem.By Taylor-series expansion, we can transform this nonlinear problem into

Algorithm 1 Navigation State Update Process Based on Factor Graph
Input: original state variable set X 0:k , measurements so far Z 1:k , threshold η Output: maximum a posterior estimation of state variable set X0:k Initialize: linear problem and get the optimal state increment estimation: where J ( X0:k ) is the sparse Jacobian matrix, 0:k is the state increment matrix till time t k , and b( X0:k ) is the residual matrix given the measurements so far.Once ˆ 0:k is calculated, the new optimal state estimate can be updated by X0:k + 0:k , which is then used as the linearization point in the next iteration.Alg. 1 presents the pseudocode for the navigation state updating algorithm based on factor graph.In Alg.

IV. EXPERIMENTS
To verify the proposed multi-sensor information fusion method's performance, we conduct two experiments.The experiment confirms the navigation accuracy of the proposed method in both small and large scenarios.The second experiment's goal is to prove that our method can achieve sensor plug and play in software, which means when some sensors are unavailable in specific environments, the proposed method can still achieve accurate navigation solutions.Our experiments were run on a single core of an Intel i5-4210 processor with a 2.40 GHz clock rate and 6 GiB of RAM.

A. DATASETS
In the first experiment, two real-world datasets are used for small scenario and large scenario, respectively: • KITTI.KITTI Vision Benchmark Suite [42]   in the small scenario, we chose a part of a residential dataset that can be downloaded from [43] carry out the first experiment.The length of the entire trajectory was about 700 m, and the driving time was 100 s.
• SX.To further test the proposed method's performance on the real-world large-scenario environment, we captured real-world data by driving a vehicle in the middle area of Shanxi province, China, and we called this dataset ''SX''.The vehicle was equipped with IMU, BDS, odometer, baro-altimeter, and a differential GPS/INS system, which provided the ground truth data.
The navigation system's hardware architecture to obtain this dataset is shown in Fig. 5, and the parameters of these sensors are shown in Table .3. The whole trajectory was about 27,383 m long, and the whole driving time was 1359 s.The first half trajectory was nearly a straight line that cannot verify the proposed method's performance properly.Thus, we chose the second half trajectory (13,582 m) containing several turns to analyze the performance.

B. ACCURACY EXPERIMENT
The Federated Filter (FF) has been widely used in integrated navigation systems, which is a near-optimal estimator for VOLUME 9, 2021  decentralized, multi-sensor data fusion [44].To verify the performance of the proposed method from the accuracy perspective, we compared the navigation results of the factor graph method and FF.This experiment is based on two datasets mentioned in Section IV-A, one for small scenarios and the other for large scenarios.According to the sensor information given in Table .2 and Table .3, we can construct the factor graph framework for these two navigation systems, as shown in Fig. 6.
1) RESULTS ON KITTI Fig. 7 presents the results of the proposed method, FF, and the ground truth in Google Road Map on the KITTI dataset.From the results, it is clear that the proposed method had a smaller error in each turning than FF.Especially in the third and fifth turning, the error in FF is increasing for a considerable period.This is caused by the uncertainty of the system model in FF during the turning periods.Furthermore, the vehicle is easily affected by the hybrid of Gaussian noise and non-Gaussian noise in the natural environment, so there are some deviations in both methods.Fig. 8 illustrates the position and velocity error of these two methods.Recall, the closer the curve is to zero, the better is the effect of the estimation.From the position error curve, we can see that FF has a more considerable fluctuation in x and y directions, while in the vertical direction, FF has a smaller error.Finally, from the velocity error curve, it is clear that FF fluctuates more in all directions.
To quantify the navigation error, we calculate root-meansquare error (RMSE) of position and velocity for these two methods, as shown in Table .4. The improvement of the factor graph method is obvious: (1) it can be seen that the factor graph method has higher position accuracy than FF in x and y directions, reducing to less than 1.5 m in RMSE.Simultaneously, in the vertical direction, FF has a smaller error; (2) as for the velocity error, the factor graph method improves in all directions.More precisely, in horizontal directions, the factor graph method's error is approximately half of FF.Thus, this experiment proves that the factor graph method can reach more precise navigation results in a small scenario than FF.

2) RESULTS ON SX
The navigation results on the SX dataset are shown in Fig. 9.As presented in the enlarged drawing of the first 100 s of the trajectory, the proposed method had a considerable drift.We believe this is because, in the proposed algorithm, we initialized the velocity of the experimental vehicle as zero, while the true velocity was not zero (recall, we start fusing   the measurements from the middle of the trajectory, not the beginning).In the rest of the trajectory, the proposed method has a slight deviation from the ground truth.
The position error of the two methods after the first 100 s is illustrated in Fig. 10 and Table .5, where we can find that the proposed method has better performance than FF.Consistent with the KITTI dataset results, in x and y directions, the factor graph method has smaller errors than FF, and its RMSE reduces to less than 2 m.Furthermore, the improvement in horizontal directions is better than the vertical direction.
From the experimental results, we can find that the SX dataset results are somewhat worse than the KITTI dataset because, in this experiment, there is no high-precise visual odometer to be combined to get the navigation result.However, we can still conclude that the proposed method also shows superiority over FF in the large scenario.

C. SENSOR PLUG AND PLAY EXPERIMENT
Since there are always cases when some particular sensors are unavailable, the navigation system needs to react quickly and recover the required navigation accuracy in a short time.This experiment uses the second dataset -SX to perform a sensor plug and play experiment.We divided the whole trajectory into five sections, and in each section, we assume there is 20 s of BDS signal loss.The exact starting and ending points of these five-time slots are shown in Table .6.We calculate  the navigation solution with BDS signal loss utilizing FF and the factor graph method.The navigation results are shown in Fig. 11.
By enlarging two navigation results in the first and third signal loss time slots (858 s to 878 s and 969 s to 989 s), we can find that the factor graph method can react in a short time and reduce navigation error quickly while FF needs more time to regain a precise result.Especially when the signal is lost, the error of FF increases immediately to a significant degree.The position error with signal loss of two methods is shown in Fig. 12, and the RMSE results are presented in Table .7. It can be seen that the performance of both methods degrades with the signal loss, but the factor graph method drops less in navigation accuracy, and it can still guarantee accuracy around 3 m.This experiment shows that: (1) for the factor graph-based method, the navigation accuracy is worse without the BDS signal at the beginning, but in general, the navigation results are still precise; (2) FF gets larger position error than the factor graph method when there are some sensors unavailable; (3) factor graph method is more effective for multi-sensor information fusion in various environments.
This experiment can also illustrate the stability of the proposed method.RMSE is a frequently used measure of the differences between values predicted by a model or an estimator, so it is reasonable to use RMSE to compare the stability of the methods.It can be seen from Table .7 that when signal losing occurs, position RMSE of the factor graph based method increases by 63.84%, 82.41%, 46.77% in eastern, northern, and vertical directions respectively, and for the federated filter, it increases by 160.65%, 172.36%, 21.60% in  the three directions respectively.Therefore, the factor graph method is more stable than FF in a complex environment.

D. TIMING PERFORMANCE
In addition to navigation accuracy, the timing performance of the proposed method is also considered to evaluate and analyze the effectiveness of the proposed method comprehensively.
The experiments of the two methods were carried out 50 times for each dataset.The average computational time is shown in the Table .8. It can be seen from Table .8 that the proposed method takes less fusion time for the navigation solution than the federated filter on both two datasets.The calculated fusion time of the two methods is shown in Fig. 13.It is obvious that the computational cost of the proposed method is much smaller than that in the federated filter.Specifically, the average fusion time of the factor graph is 62.05% and 66.16% of the federated filter on KITTI and SX datasets respectively.It can also be seen intuitively from Fig. 13 that under the situation with signal loss, the factor graph method can cost less time to recover, i.e., compared with the fusion time without signal loss, it takes 1.18% more time for factor graph based method to get the navigation solution while federated filter takes 7.27% more time.That is because once there are new navigation measurements added to the integrated navigation system, the proposed method in this paper only needs to add corresponding factors, while the federated filter needs to rebuild the algorithm model.Therefore, the proposed method not only can process data more efficiently but also have better expansibility.

V. CONCLUSION
In this paper, we study the multi-sensor information fusion method in the integrated navigation systems.To perform efficient information fusion, we proposed an approach based on the factor graph topology.The constructed factor graph framework can clearly illustrate the relationship between the navigation states and sensor measurements, making it easier to calculate the optimal navigation results with asynchronous data.By linearizing the cost function, we can also fuse nonlinear information.Furthermore, when some sensors become unavailable, the algorithm's only change is to stop adding the corresponding factors to the graph, which is an outstanding advantage compared with conventional filtering methods.Two experiments have been performed to prove the proposed method's effectiveness, and the results suggest that our proposed method outperforms the popular Federated Filter.Additionally, the sensor plug and play experiment further validates the factor graph method's good performance in a complex environment.Due to the limitations of experimental conditions, we only fuse four navigation sensors in this paper.In future work, we plan to increase the number of navigation sensors in the integrated navigation system.

FIGURE 1 .
FIGURE 1.An example of a factor graph.

FIGURE 2 .
FIGURE 2. Examples of a factor graph in inertial navigation system: (a) Factor graph with conventional IMU and bias factors; (b) An equivalent factor graph to (a) when using one equivalent IMU factor instead of three conventional IMU factors between X 1 and X 3 .

FIGURE 3 .
FIGURE 3. Example of a factor graph in INS/GPS integrated navigation system.

FIGURE 4 .
FIGURE 4. Example of a factor graph in INS/Visual Odometer integrated navigation system.

FIGURE 5 .
FIGURE 5. structure of the ground vehicle navigation system.

FIGURE 6 .
FIGURE 6.The multi-sensor fusion framework based on factor graph for two datasets: (a) The factor graph of the navigation system based on the KITTI dataset.In this framework, based on the update frequency of GPS, stereo cameras, and IMU, between navigation state X 0 and X 10 , there are two GPS factors, ten visual odometer factors, and ten equivalent IMU factors.Specifically, ten consecutive IMU measurements are combined into an equivalent IMU factor.(b) The factor graph of the navigation system based on the SX dataset.According to the update frequency of sensors in the system, between state X 0 and X 10 there are two BDS factors, ten equivalent IMU factors, one visual odometer factor, and two baro-altimeter factors.Same as the KITTI dataset, ten consecutive IMU measurements are combined into an equivalent IMU factor in this system.

FIGURE 7 .
FIGURE 7. Trajectories for the ground truth, factor graph method, and Federated Filter on KITTI dataset.

FIGURE 8 .TABLE 4 .
FIGURE 8. Comparison curves of factor graph method and Federated Filter on the KITTI dataset: (a) Position error curves; (b) Velocity error curves.

FIGURE 9 .
FIGURE 9. Trajectories for the ground truth, factor graph method, and Federated Filter on the SX dataset.

FIGURE 10 .
FIGURE 10.Position error curves of two methods on the SX dataset.

FIGURE 11 .
FIGURE 11.Navigation results of factor graph method and FederatedFilter with signal loss on the SX dataset.

FIGURE 12 .
FIGURE 12. Position error curve of two methods with signal loss on the SX dataset.

TABLE 1 .
Structure parameters of the stereo camera in the KITTI dataset.

TABLE 2 .
Measurement error of navigation sensors in the KITTI dataset.

TABLE 3 .
Measurement error of navigation sensors in the SX dataset.

TABLE 5 .
Navigation error of factor graph method and Federated Filter on the SX dataset.

TABLE 6 .
Starting and ending time of five time slots without BDS Signal on the SX dataset.

TABLE 7 .
Navigation error of two methods with/without BDS signal loss on the SX dataset.

TABLE 8 .
Time consumptions of two methods based on the two datasets.FIGURE 13.Comparison of fusion time of two methods based on KITTI dataset and SX dataset (with/without signal loss).