# Intelligent Environment-Adaptive GNSS/INS Integrated Positioning with Factor Graph Optimization
## 2 System Architecture of Adaptive GNSS/INS Integration by FGO
### Factor Graph Optimization  
In the factor graph, GNSS and INS data participate in the coupling as connected factors (i.e., \(f^{GNSS}\) and \(f^{INS}\)) of the undirected structure, as depicted in Figure 2. The example graph shows the state vector \(\mathbf{S}\) from initial epoch 1 to the final epoch \(n\), with an example section at epoch \(t\) being specially displayed.  
Figure 2: The schematics of a GNSS/INS factor graph. Circles and rectangles represent the states and factors, respectively. The example section at epoch \(t\) is displayed to illustrate formulations from (17) to (22).  
In the loosely coupled structure, the GNSS factor represents the measurement constraints between the estimated states and the measurements obtained from GNSS. At epoch \(t\), GNSS factor \(f_{t,GNSS}\) comes from the position measurement updated by the receiver with a Gaussian noise \(N(0,\Sigma_{GNSS})\) assumption. With the GNSS observation function denoted as \(h^{GNSS}(\cdot)\) and GNSS solution represented by \(Z_{t,GNSS}\), the GNSS factor can be modeled as:  
\[f_{t,GNSS}=Z_{t,GNSS}=h^{GNSS}(S_{t})+N(0,\Sigma_{GNSS}) \tag{17}\]  
where \(\Sigma_{GNSS}\) exactly represents the GNSS solution uncertainty and can be tuned to achieve both fixed and adaptive coupling. This is comparable with the measurement covariance \(R_{t}\) in KF, and the detailed settings will be specified in Section 4.1. Moreover, the discrepancy between the measurement and the prediction constitutes the error function of the GNSS factor:  
\[||\mathbf{e_{t,GNSS}}||^{2}_{\Sigma_{GNSS}}=||\mathbf{Z_{t,GNSS}}-h^{GNSS}(S_{t})||^{2} _{\Sigma_{GNSS}} \tag{18}\]  
Similar to the GNSS factor, the INS factor demonstrates the relationship between the states \(S\) and INS inputs \(\mathbf{U}\). It models the motion and enables tracking on the object's movement at all epochs, even when the GNSS signal is temporarily unavailable. Mathematically, the states \(\mathbf{S_{t}}\) at current epoch \(t\) is obtained based on the states \(\mathbf{S_{t-1}}\) and INS measurements \(\mathbf{U_{t-1}}\), which contain the processed accelerometer and gyroscope measurements in global coordinates at the previous epoch \(t-1\). The INS factor \(f_{t,INS}\) is defined as:  
\[f_{t,INS}=S_{t}=h^{INS}(S_{t-1},\mathbf{U_{t-1}})+N(0,\Sigma_{INS}) \tag{19}\]  
where \(N(0,\Sigma_{INS})\) denotes the assumed Gaussian noise model with zero mean and noise covariance of \(\Sigma_{INS}\) and \(h^{INS}(\cdot)\) is the INS observation function. Correspondingly, the error function is given as:  
\[||\mathbf{e_{t,INS}}||^{2}_{\Sigma_{INS}}=||\mathbf{S_{t}}-h^{INS}(S_{t-1},\mathbf{U_{t-1}} )||^{2}_{\Sigma_{INS}} \tag{20}\]  
Combining the above, the FGO algorithm is designed to maximize the posterior probability of the system states given the GNSS and INS information, as shown in the equation:  
\[\hat{S}=\operatorname*{arg\,max}_{S}P(S|\mathbf{Z},\mathbf{U}) \tag{21}\]  
where \(S\) is the set of state vectors, \(\mathbf{Z}\) is the set of GNSS solutions throughout all of the epochs, and \(\mathbf{U}\) is the set of INS inputs, respectively. Such an objective function can further be transformed to minimize the summation of error functions of both the GNSS and INS factors [11]:  
\[\hat{S}=\operatorname*{arg\,min}_{S}\sum_{t}(||\mathbf{e_{t,GNSS}}||^{2}_{\Sigma _{GNSS}}+||\mathbf{e_{t,INS}}||^{2}_{\Sigma_{INS}}). \tag{22}\]  
At each epoch, FGO estimates the state by conducting optimization (i.e., Equation (22)) iteratively according to the information from all historical epochs from the first epoch to the current epoch \(t\). Meanwhile, KF only employs the INS propagation from epoch \(t-1\) and the GNSS measurement at the current epoch \(t\) for estimation. With more historical information being considered in state estimation, FGO is proven to outperform KF in terms of accuracy and smoothness for the positioning solutions [11].