An Improved Multi-Sensor Fusion Navigation Algorithm Based on the Factor Graph

An integrated navigation system coupled with additional sensors can be used in the Micro Unmanned Aerial Vehicle (MUAV) applications because the multi-sensor information is redundant and complementary, which can markedly improve the system accuracy. How to deal with the information gathered from different sensors efficiently is an important problem. The fact that different sensors provide measurements asynchronously may complicate the processing of these measurements. In addition, the output signals of some sensors appear to have a non-linear character. In order to incorporate these measurements and calculate a navigation solution in real time, the multi-sensor fusion algorithm based on factor graph is proposed. The global optimum solution is factorized according to the chain structure of the factor graph, which allows for a more general form of the conditional probability density. It can convert the fusion matter into connecting factors defined by these measurements to the graph without considering the relationship between the sensor update frequency and the fusion period. An experimental MUAV system has been built and some experiments have been performed to prove the effectiveness of the proposed method.


Introduction
In recent years, the use of low-cost miniature unmanned aerial vehicles (MUAVs) for civilian applications has evolved from imagination to actual implementation. Systems have been designed for environmental monitoring, search and rescue, mapping, and mining and post seismic emergency management [1][2][3][4][5]. In order to obtain an acceptable cost/benefit ratio of these systems, there have been many techniques to reduce the cost including the aspects of the sensor, the platform, and the algorithm [6].
However, cost reduction of the sensor can lead to lower stability and accuracy, which is directly related to the reliability of the system. Thus, dependence on a single sensor cannot satisfy the demand and makes it difficult to obtain the accurate response to external environment. Through integrated usage of multiple-source information, people can get more objective and intrinsic knowledge of a certain target [7]. As a matter of fact, multi-sensor integrated navigation is becoming a hot area of research. Many studies focus on the integration of the GPS/INS system with additional sensors such as magnetometers, cameras, ultrasonic sensors, laser scanners, barometers, or earth/sun/star sensors [8]. The combination scheme can make use of all information from these sensors and overcome their limitations, which allows for the provision of reliable navigation solutions [9]. MPU-6000 inertial sensor, including a three-axis MEMS gyroscope and a three-axis accelerometer. • HMC5983 magnetometer enables 1 • to 2 • compass heading accuracy with temperature compensation. • MS5611 barometer module, with an altitude resolution of 0.1 m. • URM37 sonar module provides 0.04 m-5 m non-contact measurement function, the ranging accuracy can reach to 1 cm. • Ublox LEA 6H GPS receiver, with the position accuracy of 2 m. • Optical flow sensor processing the pixel resolution of 752 × 480 at 120 (indoor) to 250 Hz (outdoor).
The ground control station used is the advanced open-source ground Control Station software, which can realize 2/3D aerial maps with drag-and-drop waypoints. The SPI bus is used for interfacing the sensor to our microcontroller which makes the measurements available to our ground control station. The hardware structure of the MUAV is shown in Figure 1. The ground control station used is the advanced open-source ground Control Station software, which can realize 2/3D aerial maps with drag-and-drop waypoints. The SPI bus is used for interfacing the sensor to our microcontroller which makes the measurements available to our ground control station. The hardware structure of the MUAV is shown in Figure 1.

System Model for the Navigation System
In order to accomplish the navigation, the system equations of the system can be established. In this system, the local North-East-Down frame is selected as navigation frame.

State Model of the System
The state variable of the micro unmanned aerial vehicle system can be chosen as follows.

( )
where   T x y z  p represents the relative position vector to the desired hover position navigation position, represents the attitude quaternion, is the acceleration random walk term, and x y z is the gyro random walk term.
The continuous time kinematic navigation equations are given by:

System Model for the Navigation System
In order to accomplish the navigation, the system equations of the system can be established. In this system, the local North-East-Down frame is selected as navigation frame.

State Model of the System
The state variable of the micro unmanned aerial vehicle system can be chosen as follows.
where p = x y z T represents the relative position vector to the desired hover position represents the velocity vector, q = q 0 q 1 q 2 q 3 T represents the attitude quaternion, ∇ a = ∇ a x ∇ a y ∇ a z T is the acceleration random walk term, and ∇ g = ∇ g x ∇ g y ∇ g z T is the gyro random walk term. The continuous time kinematic navigation equations are given by: are the gaussian white noises.

Measurement Model of the System
In the MUAV navigation system, the measurement model should be established below.

GPS Measurement Equation
The GPS receiver measures the MUAV's velocity and position. Hence, the GPS measurement equation is described as Equation (3).
where p gps , v gps are measurements of the GPS receiver; and ω gps p , ω gps v are the gauss stochastic measurement noise terms.

Barometric Altimeter Measurement Equation
The barometric altimeter is used to measure barometric pressure and translate it into an output of voltage ranging from zero to five volts. The barometric errors vary in accordance with the environment. The observation model of the barometer sensor is given by where h pres is the altitude measured by the barometer, h is the true altitude, and ω pres is the white noise of the barometer. The height measured by the barometric altimeter is relative to the sea level, while the height measured by the GPS receiver is based on the WGS84 coordinate system. There are some differences between them. In practical application, the height has to be converted to a uniform standard and then fused. The sea level parameter correction method is often used to compensate the barometric altimeter bias. Actual sea level parameters can be calculated by using the atmospheric parameters of the location when the altitude is known. With the modified sea-level parameters, the barometric altimeter bias will be compensated.

Magnetometer Measurement Equation
The magnetometer is one of the common sensors used in MUAV. It is lightweight and reliable. It can provide the direction of the magnetic field, which can be used as aiding information. The magnetometer output m b is not influenced by the maneuvering acceleration. Thus, the well-compensated magnetometer data are used to measure the attitude matrix C b n to enhance the accuracy of the attitude estimation. The magnetometer measurement equation is described as Equation (5).
where m n is the magnet vector in navigation frame. Supposing the local magnetic declination and magnetic inclination are θ dec and θ inc respectively, then m n =    cos θ inc cos θ dec cos θ inc sin θ dec − sin θ inc   .

Optical Flow Measurement Equation
The downward-looking visual system is called the PX4-FLOW sensor, which is an open source embedded metric optical flow CMOS camera designed for indoor and outdoor applications. The raw pixel movement data are used to calculate the horizontal velocity. The observation model of the optic-flow sensor is given by v optic = kv + ω optic (7) where v optic is the velocity acquired by the optic-flow sensor, v is the true horizontal velocity, ω optic is the white noise of optic-flow observation, and k represents the coefficient determined by the camera focal length and altitude of the MUAV.

Sonar Measurement Equation
The sonar sensor and the barometric altimeter are often used to measure the altitude in the MUAV system. Because the sonar sensor has advantages of high precision, narrow measuring range and good stability, it can also meet the demand of indoor or low altitude flight. The barometric altimeter can be easily affected by environment, but it has a wide measuring range. So it can be used in the outdoor or high altitude flight. In some specific environments, these two kinds of sensors can be used in combination to acquire high reliability. The sonar measurement equation is described as Equation (8).
where y is the output of the sonar, x is the true distance, k represents the coefficient of the first order, and b is the fixed distance error. The error coefficients k and b are acquired by laboratory calibration tests.

Information Fusion Method Based on the Factor Graph
The factor graph is a graphic model that encodes the conditional probability density among unknown variable nodes and the received measurements. The global optimum solution is factorized according to the chain structure of the factor graph, which allows for a more general form of the conditional probability density. It can convert the fusion matter into connecting factors defined by these measurements to the factor graph without considering the relationship between the sensor update frequency and the fusion period.

Factor Graph Formulations
The probability graph model is a kind of graph model which can express the joint probability distribution of random variables. The factor graph is a bipartite graph representing the factorization structure of a multivariate function into a product of functions (factors), each involving only a subset of the variables [26]. There are two kinds of nodes in the graph. The variable node X represents the variable of the global multivariate function, and the factor node F represents the factor in the factorization. The variable node is connected to a factor node by an edge E [27]. The factor graph is defined as where X i ⊆ {x 1 , x 2 , · · · , x m } is the subset of the variable nodes x j (j = 1, 2, ..., m), m is the number of the variable node, .., f n (X n )} represents a set of the factor nodes f i (X i )(i = 1, 2, ..., n), and n is the number of the factor node. Edges E = e ij denotes the set of links of the factor graph, which can exist only between factor nodes and variable nodes. The factor graph G defines one factorization of the function as A factor graph can be used as a probabilistic graphical model which represents a joint probability mass function of random variables. Each variable node represents a set of the random variables. Each factor represents the joint probability function of a subset of random variables, which is shown as a box or a circle. For example, Figure 2 can be factored as (11) variable of the global multivariate function, and the factor node F represents the factor in the factorization. The variable node is connected to a factor node by an edge E [27]. The factor graph is defined as , and n is the number of the factor node. Edges

 
ij E e  denotes the set of links of the factor graph, which can exist only between factor nodes and variable nodes. The factor graph G defines one factorization of the function as A factor graph can be used as a probabilistic graphical model which represents a joint probability mass function of random variables. Each variable node represents a set of the random variables. Each factor represents the joint probability function of a subset of random variables, which is shown as a box or a circle. For example, Figure 2 can be factored as The factor vertex f1, f2, f3 are the local functions. Due to the variable vertex u, w, x are connected to f1, the factor vertex f1 involves the independent variables u, w, x. In a similar way, the factor vertex f2 involves the independent variables x, y, z. The factor vertex f3 involves the independent variable z.

Fusion Algorithm with the Factor Graph
For our application, the factor graph framework model is established for the MUAV navigation system. This graph-based optimization algorithm is particularly developed to solve the optimization problem for the current state prediction with asynchronous multi-sensor data. The main purpose is to calculate the best navigation solution by using the available information sources.
The state variable of the MUAV system X is described in Section 3.1.
represents the independent measurement in the The factor vertex f 1 , f 2 , f 3 are the local functions. Due to the variable vertex u, w, x are connected to f 1 , the factor vertex f 1 involves the independent variables u, w, x. In a similar way, the factor vertex f 2 involves the independent variables x, y, z. The factor vertex f 3 involves the independent variable z.

Fusion Algorithm with the Factor Graph
For our application, the factor graph framework model is established for the MUAV navigation system. This graph-based optimization algorithm is particularly developed to solve the optimization problem for the current state prediction with asynchronous multi-sensor data. The main purpose is to calculate the best navigation solution by using the available information sources.
The state variable of the MUAV system X is described in Section 3.1. X k ∈ R n is the state variable in the current time t k . Given the initial distribution p(X 0 ), X k can propagate with the probability density p(X k |X k+1 ). Z k ∈ R m represents the independent measurement in the current time t k . The Bayesian estimation contains two parts, the prediction and the update. The prediction concerns the prior density of the state with the state model. The update process is to modify the prior density obtained by the previous step by using the newest measurement in order to obtain an a posteriori probability density p(X 0:k |Z 1:k ). Our goal is to solve the a posteriori probability density in the navigation system [28].
Given the observations up to time k, the global conditional joint probability density function p(X 0:k |Z 1:k ) for the state vector X 0:k is the marginal function. According to the Bayes formula, p(X 0:k |Z 1:k ) can be factorized as: where p(X 0 ) represents all of the available prior information. This global conditional probability density function is proportional to the likelihood probability density and the state transition prior probability in the numerator.
According to the maximum a posteriori criterion, the state variable with the maximum a posteriori probability density is considered as the estimation.
Equation (12) can be written as To calculate theX MAP i in Equation (14), the right side of Equation (15) will be maximized. For Gaussian noise distributions, the probability density p(Z i |X i )can be calculated as: where W is the variance matrix, h i () is measurement function, and Z i is the actual measurement. For the above-mentioned reasons, Equation (17) should be minimized as the following function For linear measurement, the optimal solutionX i can be solved by using the method of seeking extreme. For nonlinear measurement, it can be solved by using Newton iteration method. When adding the new factor node Z x,i to the graph, the measurements of different sensors are used to calculate the optimal solution.
By the similar method, the probability density p(X i |X i−1 ) can be calculated as the same way. The probability density p(X i |X i−1 ) can be calculated as where P is the variance matrix. Equation (19) should be minimized as the following function: The estimation of the stateX i/i−1 can be solved by state vectors propagating. When adding the new variable X i+1 to the graph, the IMU measurement is used to calculate the state transition matrix to predict the values for X i+1 . In the factor graph. Let f () represent the local functions of the probability density function. Equation (12) can be written as where f (Z i |X i ) and f (X i |X i−1 ) are the local functions associated with the factor nodes P x,i and Z x,i in Figure 3, respectively.
P P (18) where P is the variance matrix. Equation (19) should be minimized as the following function: The estimation of the state  (12) can be written as  The factor graph model of the navigation system is dominated by the two local functions By connecting the variable nodes associated with each function to the factor node, the factor graph can be constructed. The factor graph method allows for a more general form of the conditional probability density. The factor graph model of the navigation system is dominated by the two local functions f (Z i |X i ) and f (X i |X i−1 ). By connecting the variable nodes associated with each function to the factor node, the factor graph can be constructed. The factor graph method allows for a more general form of the conditional probability density.

Factor Graph Modeling
Not all of the sensors can be updated at the same time. In general, the frequencies of the IMU measurements are very high, which are different from other sensors. For example, the update frequency of GPS measurement is slower than IMU, which can be described in Figure 4.

Factor Graph Modeling
Not all of the sensors can be updated at the same time. In general, the frequencies of the IMU measurements are very high, which are different from other sensors. For example, the update frequency of GPS measurement is slower than IMU, which can be described in Figure 4.  f GPS denotes the factor node when the system receives the measurement , ( ) Z x i GPS . The factor GPS is defined as the following form Variable node X i represents the state variable of the system. f x,i (I MU) denotes the updating of the variable node with the IMU measurement P x,i (I MU). The factor IMU is defined as the following form It connects two different variable nodes X i and X i+1 in t i and t i+1 moments, respectively. The IMU measurement is used to calculate the state transition matrix to predictX i+1/i by state vector propagation.
On the other hand, f x,i (GPS) denotes the factor node when the system receives the measurement Z x,i (GPS). The factor GPS is defined as the following form The estimation of X k can be solved by using the method of seeking extreme through the probability density function p(Z GPS i |X k ). It can be calculated as Equations (16) and (17). With this model, we can calculate the minimization of the local functions f (Z i |X i ) and f (X i+1 |X i ) to acquire the optimal state at t i moment.
When the system receives the magnetic measurement Z x,i+1 (Magnetic sensor), the factor node f x,i+1 (Magnetic sensor) will be added into the graph. Because the update frequency of the magnetic sensor is faster than that of GPS, the magnetic sensor nodes appear many times during the update of GPS. It is described in Figure 5. will be added into the graph. Because the update frequency of the magnetic sensor is faster than that of GPS, the magnetic sensor nodes appear many times during the update of GPS. It is described in Figure 5.  The update frequencies of different sensors in the MUAV are different. In a similar way, the factor graph model describes the characteristics of the MUAV navigation system, which are shown in Figure 6. The update frequencies of different sensors in the MUAV are different. In a similar way, the factor graph model describes the characteristics of the MUAV navigation system, which are shown in Figure 6. This model enables us to handle situations where different sensors provide measurements at different times and at different time intervals. When the measurement equation of the new sensor is nonlinear, the method can simply add new factors to the graph. If a sensor becomes unavailable due to various reasons, the system simply stops adding the associated factors to the graph. There is no need for a special procedure.
To implement the factor graph method, the steps are introduced as follows: Step  This model enables us to handle situations where different sensors provide measurements at different times and at different time intervals. When the measurement equation of the new sensor is nonlinear, the method can simply add new factors to the graph. If a sensor becomes unavailable due to various reasons, the system simply stops adding the associated factors to the graph. There is no need for a special procedure.
To implement the factor graph method, the steps are introduced as follows: Step 1: Set the initial parameters and define a state-space vector. New factors f new = {} and new variables X new = {} are initialized. The probability density function p(X 0 ) should be set up according to the parameters of the system.
Step 2: When the system receives the IMU measurement f b , w b ib , at t i moment, the factor node P x,i (I MU) will be added into the graph. It connects two different variable nodes X i and X i+1 in t i and t i+1 moments, respectively. The IMU measurement is used to calculate the state transition matrix to predictX i+1/i by state vector propagation.

•
Position is calculated by Step 3: Step 4: When the system receives the measurement Z x,k (magnetic, GPS, sonar or optic flow, etc.) at t k moment, the factor node f x,k (magnetic, GPS, sonar or optic flow, etc.) will be added into the graph. Add f x,k to f new = {}.
Step 5: The optimization problem encoded by the factor graph is solved by Gauss-Newton iterations.
Z is the set of all measurements, and X represents the set of all variables.X is an initial estimate of X. According to Equation (17), the increment ∆X needs to be calculated, which should satisfy Equation (23). argmin where J(X) is the Jacobian matrix and r(X) is the residual of all measurements.
Calculating the increment ∆X requires factoring the Jacobian matrix into an equivalent upper triangular form, such as QR factorization. Once the increment ∆X is calculated, the new estimatê X + ∆X can be obtained. It is set to be the initial estimate in the next iteration. For an example of the system, the factor graph in Figure 7a contains the following four factors which are expressed in the box. The system in Figure 7b adds a new factor, a block row is added in the Jacobian matrix, which is denoted as ×. Calculating the increment X requires factoring the Jacobian matrix into an equivalent upper triangular form, such as QR factorization. Once the increment X is calculated, the new estimate ˆ  X X can be obtained. It is set to be the initial estimate in the next iteration. For an example of the system, the factor graph in Figure 7a contains the following four factors which are expressed in the box. The system in Figure 7b adds a new factor, a block row is added in the Jacobian matrix, which is denoted as  . The square root information matrix R in Figure 7a is as follows, which is obtained by QR factorization of the Jacobian matrix: The square root information matrix R in Figure 7a is as follows, which is obtained by QR factorization of the Jacobian matrix: When the new factor node is added in Figure 7b, the corresponding square root information matrix R' can be changed as follows: The modified value, which is different from R, is denoted as ×. It can be seen that the first two block rows remain unchanged. So, there is no need to recalculate these values.
Step 6: Determine whether the new measurement is received by the system. Return to Step 2.

Simulation and Analysis
In order to verify the performance of the factor graph method proposed in this paper, the simulation and analysis are carried out with different methods. In the simulation, a typical flight trajectory has been designed. We assume a MUAV equipped with many sensors, including IMU, GPS receiver, magnetometer, and barometer. IMU sensors produce measurements at a high rate while other sensors generate measurements at lower rates. The flight time is 600 s, and the initial position is [106.5 • , 29.53 • , 25 m]. Parameters of navigation sensors are shown in Table 1. The extended Kalman filter (EKF) has been used widely in integrated navigation systems, which linearizes all nonlinear models. In the traditional EKF algorithm, the state propagates with the IMU update rate, and performs a measurement update whenever the measurements are available. The factor graph method is compared with traditional EKF method in the simulation. The flight track is shown in Figure 8. The five-pointed star represents the starting point. The flight altitude remains the same.
Monte-Carlo simulations have been performed and the RMSE performances are used to compare the accuracy of different algorithms. After 100 Monte Carlo simulations, the average RMSE of both methods are given in Table 2. Compared with the ground truth data from the flight track, Figure 9a,b illustrates the average RMSE of the position error and the velocity error generated by these two algorithms, respectively.
The extended Kalman filter (EKF) has been used widely in integrated navigation systems, which linearizes all nonlinear models. In the traditional EKF algorithm, the state propagates with the IMU update rate, and performs a measurement update whenever the measurements are available. The factor graph method is compared with traditional EKF method in the simulation. The flight track is shown in Figure 8. The five-pointed star represents the starting point. The flight altitude remains the same.  Table 2. Compared with the ground truth data from the flight track, Figure 9a,b illustrates the average RMSE of the position error and the velocity error generated by these two algorithms, respectively. From the above simulation results, the average RMSE performances of the EKF and the factor graph filter are compared in Table 2. The improvement of the factor graph filter is obvious. The corresponding accuracy in the velocity and position of the factor graph method improve as well.  From the above simulation results, the average RMSE performances of the EKF and the factor graph filter are compared in Table 2. The improvement of the factor graph filter is obvious. The corresponding accuracy in the velocity and position of the factor graph method improve as well.

MUAV Flight Test and Results
Outdoor autonomous flight tests on real data have been carried out in a playground. All of the sensors described in Section 2 are mounted on the vehicle. To evaluate the navigation performance, a more precise navigation system which has a differential GPS/INS system on the autopilot board is used as the reference system. For the multi-sensor navigation, GPS positioning is used in single point mode. We make use of the ARM processor with highly optimized C language to ensure real-time performance. The data are captured from the autopilot board of the MUAV during the flight. The total flight time was about 500 s.
The MUAV system in the test flight experiment is shown in Figure 10a. Autonomous flights include takeoff, waypoint, returning, and landing modes. The horizontal flight track compared with the expected route is presented in Figure 10b. System dynamic parameters are shown in Table 3.  Figure 10a. Autonomous flights include takeoff, waypoint, returning, and landing modes. The horizontal flight track compared with the expected route is presented in Figure 10b. System dynamic parameters are shown in Table 3.    The expected route with four waypoints is the pre-set track which is uploaded in the MUAV system before taking off. The circles of the four corners in the square represent the waypoints, and the default waypoint radius is configured to 3 m. This means that if the vehicle reaches the waypoint within the default waypoint radius, it will turn to the next waypoint. The flight track is obtained by the data of the MUAV navigation system. It can be seen that the vehicle can fly along the expected route. Figure 11a,b illustrates the performance of the position error and the velocity error generated by these two filters. The position and velocity errors of the factor graph filter and EKF are compared in Table 4. It can be seen that the factor graph method has higher precision than EKF. The RMSE results reduce to less than 80%. Better results of the system by using the factor graph filter method can be obtained, and the accuracy level can meet the requirements of the MUAV navigation system. The position and velocity errors of the factor graph filter and EKF are compared in Table 4. It can be seen that the factor graph method has higher precision than EKF. The RMSE results reduce to less than 80%. Better results of the system by using the factor graph filter method can be obtained, and the accuracy level can meet the requirements of the MUAV navigation system.

Conclusions
In this paper, a multi-sensor information fusion method based on the factor graph topology is proposed. The global optimum solution is factorized according to the chain structure of the factor graph. It can convert the fusion matter into connecting factors in the factor graph, which allows for a more general form of the conditional probability density. It can convert the fusion matter into connecting factors defined by these measurements to the factor graph without considering the relationship between the sensor update frequency and the fusion period. According to the factor graph theory, the maximum a posteriori probability density is derived in the form of the factor node and the variable node. By selecting the appropriate cost function of these nodes, the optimal navigation solution of the system can be calculated. An experimental MUAV system has been built, and some experiments have been performed to prove the effectiveness of the proposed method.