An Improved INS Factor Graph Formulation and Its Application in Integrated Navigation System

. A new method of establishing INS factor graph is proposed to deal with the serious waste of INS measurements in current integrated navigation factor graph algorithm. The INS observations pre-integrated to construct a smart factor during a time period, are used to solution simultaneously, to get the navigation states estimation. Then the estimated values can be used to construct an INS factor, adding into the INS factor graph. As a result, the new model is larger than the original, but the incremental inference technology makes sure the real-time ability of the factor graph algorithm. To analysis the performance of the proposed algorithm, INS and GNSS data are simulated, and the navigation position error of either the INS factors are added or not are compared. The results show that, the precision and stability of the factor graph algorithm are greatly improved by adding the INS factors. The proposed algorithm makes full use of the INS accuracy, and reflects the advantage of INS high stability.


Introduction
Navigation and positioning technology which plays an important role in precision strike and cooperative combat is an indispensable key technology in modern warfare. Presently, navigation means has been developed from the use of a single navigation method to the stage of integrated navigation system. With the increasingly harsh environment and the higher demand on the performance of navigation and positioning, the type and number of sensors integrated together are increasing day and day. Adaptive multi-sensor integrated navigation system [1] and All Source Positioning and Navigation System [2] have become an important tendency of the future navigation system.
The integrated navigation algorithm based on factor graph can flexibly integrate information of a variety of asynchronous sensors. It is very convenient to deal with the dynamic changes of the sensors combination and has great advantages in the application of multi-sensor integrated navigation system [3,4].
As a very important sub-system of integrated navigation system, INS is a processing difficulty of the integrated navigation algorithm based on factor graph, because its performance depends on the inertial sensors' high frequency measurement of the carrier's movement. However, if the factor nodes of INS are added to the factor graph by such a high frequency, the factor graph will be very large and leads to an unacceptable computational cost in practical application. What's more the navigation information does not need to be output by the INS measurement frequency. All above makes it meaningful and acceptable to reduce the INS factor graph through a reasonable method. In the current factor graph algorithm, INS measurements during a time period are pre-integrated to construct an equivalent INS factor [5]. But the pre-integration process leads to a serious waste of INS precision, because it is too simple comparing with the very mature INS solution algorithm.
In view of this, in order to fully utilize INS measurement information and ensure the real-time performance of factor graph algorithm, a new method of establishing INS factor graph is proposed. The pre-integrated INS observations are used to solution simultaneously, to obtain navigation states estimation. Then the estimated values can be used to construct an INS factor and added into the factor graph to realize a stably and accurately fusion with GNSS information.
The remaining part of this paper is organized as follows. The integrated navigation algorithm based on factor graph is introduced in Section 2. Section 3 presents the factor graph of INS/GNSS integrated navigation system. Simulation and results are provided in Section 4, and concluding remarks are suggested in Section 5.

Integrated navigation algorithm based on factor graph
The problem solved by integrated navigation algorithm is computing navigation states based on the observations. Essentially, the joint probability distribution function (pdf) is constructed by the navigation states and observations, then the maximum a posteriori (MAP) estimate is calculated and obtaining the optimal estimation of the navigation states. In factor graph algorithm, the joint pdf is described as a factor graph, and the MAP estimate is calculated in the established factor graph.

Factor graph
In integrated navigation system, we define the sets of all navigation states and calibration states up to the current time as k V , and letting k Z represents all the measurements received up to the current time. Then, the joint pdf can be written as: The optimal estimation of the navigation states in the form of MAP estimate is given by: The joint pdf (1) can always be factorized in terms of measurement models, and expressed as a factor graph. Formally, a factor graph is a bipartite graph with two node types: variable nodes j v and factor nodes i f , where j v is navigation states or calibration states, and i f is an error function corresponding to the measurement model, as: where the operator ( ) The factor graph encodes the relationship between the navigation states and the measurements of navigation systems. Defining the measurements of navigation systems as factors, the integration of navigation systems becomes to connecting these factors to the corresponding variable nodes in the factor graph. The defined factor will be added into the factor graph, when the corresponding subsystem is valid, and denied as soon as it's invalid. Such a representation provides a plug and play capability, since no special adjustment will be needed when the sub-systems changed, which greatly improves the stability of the integrated navigation system.

Incremental inference of factor graph
The basic inference method is firstly obtaining Bayes net by eliminating the factor graph. Then the chordal Bayes net can be converted into a tree-structured graphical model, namely Bayes tree. As a result, optimization and marginalization will be done in the Bayes tree.
The approach using Bayes tree to realize incremental inference [6] of the factor graph is shown in Fig. 1. When new factors are added into the graph, the affected part of the Bayes tree and the involved variables are identified, firstly. Then the affected part of the Bayes tree is converted back into a factor graph, the new factors are added, and the resulting (small) factor graph is re-eliminated to get a new (small) Bayes tree. The new (small) Bayes tree is then merged with the unchanged part of the original Bayes tree, and the incremental update of the Bayes tree is accomplished finally. Additionally, it is necessary to consider the linearization problem of the factor graph in this process. A re-linearization strategy is employed to reduce the linearization computation. When the deviation of its current estimate from the linearization point becoming larger than a threshold, the given variable will need to be re-linearized. Then all the relevant information related to this variable has to be replaced by re-linearizing the corresponding original factors. The variables need to be relinearized are also included in the affected part in Fig. 1.
New factors and re-linearization variables only update the top of the Bayes tree, however, estimating of the variables in all the sub-trees will be changed. But the farther away from the top of the tree, the less the influence, because of the effect of the new measurements is usually limited. Therefore, in order to reduce computational cost, only partial variables being affected significantly will be updated. The solve starts from the top of the tree, and will be stopped when comes to a clique, in which all the variables changed smaller than a threshold.

Factor graph of INS/GNSS integrated navigation system
The integration of INS/GNSS is a typical representative of integrated navigation system and also the core of multi-sensor integrated navigation system. So it is significant to study the application of factor graph algorithm in INS/GNSS integrated navigation system.

INS factor graph formulation
As a premise, an INS factor graph is established based on the state equations, and then an equivalent INS factor graph is proposed to reduce its size through a reasonable method.

INS factor graph
The discrete representation of INS state equations can be written as: where x represents the navigation state variables, which include position, velocity and attitude angles, D represents the INS calibration parameters, and z is the measurements of inertial sensors.
The state equations can be used to predict k+1 x and k+1 D . Then error functions represented by the factors can be given by the differences between these predictions and the current estimates: where IMU f is an IMU factor connecting k x , k+1 x and k α ; bias f is a bias factor connecting k α and k+1 α .

Equivalent INS factor graph
The navigation states only need to be output by a low frequency, while the high frequency measurements are necessary to realize the accurate solution of INS. The variables need to be output are defined as target variables, and must be added to the factor graph as variable nodes. The remaining variables, that we call support variables, are not strictly required as output of the estimation problem. But these support variables are essential for the formulation of the optimization problem, and need to be considered when constructing factor nodes of the target variables. Then, these factor nodes can be called smart factors [7]. The idea of smart factor applies to all types of navigation sensors. For INS, the smart factor is obtained by pre-integrating all the INS measurements between two target variables. However, compared to the state-of-the-art solution method of INS [8], the pre-integration process presents rough and ready, which results in a serious waste of INS precision. Therefore, The INS observations are used to calculate navigation states estimation by INS solution method, simultaneously with the pre-integration process. Then, the difference between these estimates and the current estimates defines an INS factor: Fig. 2(b) illustrates the equivalent factor graph of INS. Equiv f is the smart factor obtained by preintegration [3], and the frequency of adding variable nodes and factor nodes in the model depends on the time period of pre-integration. What's more, the equivalent INS factor graph constructed by preintegration only, can be acquired by removing the INS factors.

GNSS factor graph formulation
The GNSS measurement equation can be written as: where GNSS h is the measurement function, relating between the measurement GNSS k z to the carrier's position, GNSS n is the measurement noise. Eq. (7) defines a unary factor GNSS f : The factor graph of INS/GNSS integrated navigation system shown in Fig. 3 can be established by adding the GNSS factors to the equivalent factor graph of INS.

Simulation and analysis
The prosed method is simulated in MATLAB to compare navigation results of the model with INS factors and without INS factors. For the factor graph model shown in Fig. 3, the periods of Equiv f and INS f are both 1 s, and the period of GNSS f is 10 s. Then the incremental inference technology which makes sure the real-time ability of the algorithm is used to calculated navigation results. The comparison of position errors is shown in Fig. 5, and the Max and RMS of position errors are shown in Table 1. Obviously, the position precision of the factor graph integrated navigation algorithm is significantly improved in north and east by adding INS factor. The maximum position error of north and east are reduced by 62%, and 43%, respectively. And the position error RMS of north and east are reduced by 17% and 27%, respectively. The precision and stability of the integrated navigation algorithm based on factor graph can be improved significantly by adding the INS factors.

Conclusion
Studying the integrated navigation algorithm based on factor graph, and a new method of establishing INS factor graph is proposed. Although the model size is increased, the incremental inference technology of factor graph makes sure the real-time ability of the algorithm. The simulation results show that the proposed INS factor graph presents superior performance when integrated with GNSS.
Compared with the original model without INS factors, the new model makes full use of the INS accuracy, and reflects the advantage of INS's high stability. As a result, the precision and stability of the factor graph algorithm are both greatly improved.