An Adaptive Fast Incremental Smoothing Approach to INS/GPS/VO Factor Graph Inference

Abstract

In response to asynchronous and delayed sensors within multi-sensor integrated navigation systems, the computational complexity of joint optimization navigation solutions persistently rises. This paper introduces an adaptive fast integrated navigation algorithm for INS/GPS/VO based on factor graph. The factor graph model for INS/GPS/VO is developed subsequent to individual modeling of the Inertial Navigation System (INS), Global Positioning System (GPS), and Visual Odometer (VO) using the factor graph model approach. Additionally, an Adaptive Fast Incremental Smoothing (AFIS) factor graph optimization algorithm is proposed. The simulation results demonstrate that the factor-graph-based integrated navigation algorithm consistently yields high-precision navigation outcomes even amidst dynamic changes in sensor validity and the presence of asynchronous and delayed sensor measurements. Notably, the AFIS factor graph optimization algorithm significantly enhances real-time performance compared to traditional Incremental Smoothing (IF) algorithms, while maintaining comparable real-time accuracy.

1. Introduction

Recently, multi-sensor integrated navigation has emerged as the primary approach for achieving long-term, high-precision navigation in diverse and complex environments [1,2,3,4]. Navigation systems, including the Inertial Navigation System (INS), Global Positioning System (GPS), and Visual Odometer (VO), find widespread applications in unmanned aerial vehicle navigation systems. The INS boasts high short-term accuracy, robust autonomy, comprehensive data, frequent data updates, and impressive resistance to interference. However, it is prone to accumulating and diverging positioning errors over time [5,6]; GPS offers high positioning accuracy, yet the functioning of GPS receivers is significantly influenced by carrier maneuvers, susceptible to interference, and subject to human control; VO estimates carrier pose changes using visual information at consecutive instances, effectively mitigating the accumulation of INS errors in the absence of GPS. Consequently, achieving long-term, high-precision, and stable navigation is feasible by leveraging information from multiple navigation devices to create an integrated navigation system [7,8,9,10,11].

In general, the integrated navigation system comprises multiple navigation subsystems. Divergences in data frequency among various navigation sources and delays in the arrival of navigation data at the fusion center, attributed to the distinct working principles of each navigation method, necessitate heightened demands on information fusion methodologies within integrated navigation. Presently, the predominant information fusion algorithms for integrated navigation heavily depend on the Kalman filter (KF) and have undergone numerous refinements to enhance navigation accuracy and stability. Furthermore, the KF mandates the establishment of a state space model for the integrated navigation system. Adjustments to this model are required when the efficacy of sensors in the integrated navigation system undergoes changes, markedly amplifying the intricacy of algorithmic design [12,13,14] and constraining the potential utilization of more effective sensor information to enhance navigation performance. Additionally, KF generally confines its estimation to the current state at the present time, and the formulated measurement equation solely delineates the mapping relationship between measurement information and the current state. Complexities arise in accommodating measurement information, such as VO, which characterizes constraint relationships between diverse states at different times and proves challenging to integrate within the information fusion framework based on KF.

The factor graph represents a category of probability graph models tasked with encoding the probability relationships between navigation state variables and measurements obtained from navigation systems. The state variables are denoted as variable nodes, while each sensor measurement is identified as a factor node. The integration of measurements from diverse sensors, potentially operating asynchronously, necessitates the establishment of connections between the factors derived from these measurements and the corresponding nodes within the factor graph [15,16]. The factor graph model exhibits notable flexibility, enabling a plug-and-play functionality for sensors, a feature that has garnered considerable attention in the field of navigation [17,18,19]. In contrast to state estimation methods in filter-based integrated navigation systems, factor-graph-based integrated navigation system state estimation methods abstain from marginalizing historical states. Instead, they undertake the estimation of all states within the integrated navigation system, a task akin to solving a nonlinear least squares problem.

Batch optimization is extensively employed for information fusion in integrated navigation systems [20,21,22]. However, as batch optimization recalculates all variables of the nonlinear least squares problem, computational complexity escalates over time. In reference [23], the fixed-lag smoothing algorithm is implemented to address this issue by maintaining several past states in a sliding window. To sustain a consistent and manageable computational cost, all variables within the sliding window undergo recalculation upon obtaining a new measurement. This operation, however, proves unnecessary since some states remain unchanged. The fixed-lag smoothing algorithm is deemed sub-optimal in comparison to batch optimization unless the smooth lag is sufficiently extensive. Furthermore, the fixed-lag smoother marginalizes variables outside of the smoothing lag, posing challenges for the navigation system in establishing effective closed-loop factors. This compromises the outstanding performance of factor-graph-based integrated navigation.

The incremental smoothing (IF) technology is employed to efficiently and swiftly compute the complete navigation solution [24,25,26,27]. In contrast to batch optimization and fixed-lag smoothing, the incremental smoothing algorithm updates only a subset of nodes altered in the factor graph during each iteration. However, the incremental smoothing algorithm incrementally computes the system’s square root information matrix and optimizes all state variables at each iteration. Despite its faster calculation speed compared to batch optimization, the dimension of state variables expands with the progression of navigation time, inevitably amplifying the computational complexity for each global optimization.

Given the above analysis, the processing of asynchronous and delayed measurements in traditional KF-based integrated navigation methods is relatively complex. Additionally, the current factor graph algorithms (including batch optimization, fixed-lag smoothing, and incremental smoothing) cannot meet the accuracy and real-time demands of integrated navigation systems. To address the mentioned problems, this paper proposes an improved INS/GPS/VO adaptive fast integrated navigation algorithm based on factor graph. The main contributions of this paper are as follows:

(a)

A novel factor-graph-based INS/GPS/VO integrated navigation model is formulated, which exhibits commendable efficacy in managing dynamic alterations in sensor effectiveness, as well as accommodating asynchronous and delayed sensors;

(b)

The adaptive fast incremental smoothing algorithm is proposed, which demonstrates superior performance compared to the traditional incremental smoothing approach.

This paper is organized as follows. The proposed factor graph model for the INS/GPS/VO integrated navigation system is presented in Section 2.1. The AFIS factor graph optimization algorithm is derived in Section 2.2. The simulation results and analysis are shown in Section 3. The discussion is shown in Section 4. The conclusions are drawn in Section 5.

2. Materials and Methods

2.1. Factor Graph Model of INS/GPS/VO Integrated Navigation System

2.1.1. INS Factor Graph Model

The efficacy of INS heavily relies on the high-frequency measurement of carrier motion states facilitated by the Inertial Measurement Unit (IMU). This frequency is typically considerably higher than the output frequency of the navigation system. Consequently, the INS measurement data contain a substantial amount of redundant information compared to the navigation system output. Therefore, the idea of equivalent factors can be adopted [28] when establishing the INS factor graph model. State variables requiring output are introduced as variable nodes. The IMU measurement data between consecutive time points of adjacent variable nodes is integrated and defined as equivalent factor nodes. This approach serves to reduce the update frequency of factor nodes, thereby enhancing the real-time performance of the algorithm.

Figure 1 shows the construction scheme of the INS factor graph model. First, the output frequency of navigation state variables is determined based on the actual system application requirements. Subsequently, the update period for equivalent factors is selected. Navigation state variables earmarked for output, identified as target variables, are incorporated as variable nodes into the factor graph. Additionally, the inertial sensor bias variable is introduced to the variable node to rectify the measurement error of the inertial sensor in IMU. The bias factor node is defined using the error propagation function. During an update period, the IMU measurement data is utilized to establish a pre-integration factor node, connecting it to the corresponding variable nodes in the factor graph.

The INS functions as a dead reckoning system, amalgamating measurements of acceleration and angular velocity to derive information regarding the position, velocity, and attitude of the carrier. Utilizing the pre-integration method enables the synthesis of IMU data within an update cycle, establishing equivalent factor nodes. The integrated navigation system leverages auxiliary navigation methods to swiftly accomplish the initial alignment of the INS, facilitating the acquisition of initial position, velocity, and attitude information of the carrier. Subsequently, by directly pre-integrating acceleration and angular velocity measurements within the body frame, significant reductions in computational complexity and improvements in the real-time performance of the algorithm are achieved [29]. The output time of the navigation state is denoted by $t_k$, $k=0,1,\dots ,$ and consequently, the equivalent factor update period will be $T=t_{k+1}-t_k$. By pre-integrating IMU measurement data within an update period $t_k\sim t_{k+1}$ under the body frame, the calculation formula for position, velocity, and attitude increments is obtained as

$$\left\{\begin{array}{l}\Delta p_{t_{k+1}}^{t_k}={\displaystyle {\iint }_{t_k}^{t_{k+1}}\left(C_{bt}^{b{t}_k}\left(f_t^b-{\nabla }_f\right)\right)d{t}^2}\\ \Delta v_{t_{k+1}}^{t_k}={\displaystyle {\int }_{t_k}^{t_{k+1}}\left(C_{bt}^{b{t}_k}\left(f_t^b-{\nabla }_f\right)\right)}dt\\ \Delta {\varphi }_{t_{k+1}}^{t_k}={\displaystyle {\int }_{t_k}^{t_{k+1}}\left(E_{bt}^{b{t}_k}\left({\omega }_t^b-{\nabla }_{\omega }\right)\right)}dt\end{array}\right.$$

(1)

where $C_{bt}^{b{t}_k}$ and $E_{bt}^{b{t}_k}$ are the rotation matrix and rotation rate matrix from the body frame of current time to the body frame of time $t_k$; $f_t^b$ and ${\omega }_t^b$ are the specific force and the angular acceleration in the body frame; ${\nabla }_f$ and ${\nabla }_{\omega }$ are the bias variables of the accelerometer and gyroscope. According to the principle of INS dead reckoning, the carrier’s position, velocity, and attitude at the time $t_{k+1}$ can be further obtained as

$$\left\{\begin{array}{l}{p}_{t_{k+1}}^n=p_{t_k}^n+\left(t_{k+1}-t_k\right)v_{t_k}^n+\frac{1}{2}{\left(t_{k+1}-t_k\right)}^2{g}^n+C_{b{t}_k}^n\Delta p_{t_{k+1}}^{t_k}\\ v_{t_{k+1}}^n=v_{t_k}^n+\left(t_{k+1}-t_k\right)g^n+C_{b{t}_k}^n\Delta v_{t_{k+1}}^{t_k}\\ {\varphi }_{t_{k+1}}^n={\varphi }_{t_k}^n+C_{b{t}_k}^n\Delta {\varphi }_{t_{k+1}}^{t_k}\end{array}\right.$$

(2)

where $g^n$ is gravity vector.

Equation (2) is the discrete form of the INS system state equation, which can be uniformly expressed as

$$\begin{array}{l}{\widehat{x}}_{k+1}=h\left(x_k,{\alpha }_k,z_k^{IMU}\right)\\ {\widehat{\alpha }}_{k+1}=g\left({\alpha }_k\right)\end{array}$$

(3)

where $x_k$ represents the navigation state variable, including position, velocity, and attitude angle; ${\alpha }_k$ represents the bias variable of the inertial sensor; $z_k^{IMU}$ represents the measured value of the inertial sensor; ${\widehat{x}}_{k+1}$ is the predicted value of the navigation state value obtained from the state equation at time $t_{k+1}$; Conceptually, each equation in (3) defines distinct factors: an IMU factor $f^{IMU}$ linking the navigation nodes $x_k$, ${\widehat{x}}_{k+1}$ and the bias node ${\alpha }_k$, alongside a bias factor $f^{Bias}$ connecting the bias nodes ${\alpha }_k$ and ${\widehat{\alpha }}_{k+1}$. In practice, consecutive IMU measurements are amalgamated into a singular factor, as depicted in Figure 2. Nonetheless, delineating explicit expressions for the conventional IMU factor $f^{IMU}$ remains beneficial.

The conventional IMU factor $f^{IMU}$ corresponding to a specific IMU measurement $z_k^{IMU}$ is defined as follows. The IMU measurement $z_k^{IMU}$ and the current estimate of the state $x_k$, ${\alpha }_k$ are utilized to forecast the values for the state ${\widehat{x}}_{k+1}$. The discrepancy between this prediction and the current estimate $x_{k+1}$ is encapsulated as the error function represented by the factor

$$f_{k+1}^{IMU}\left(x_{k+1},x_k,{\alpha }_k\right)\doteq d\left(x_{k+1}-h\left(x_k,{\alpha }_k,z_k\right)\right)$$

(4)

where the operator $d\left(·\right)$ denotes a specific cost function. Upon the addition of the new variable node $x_{k+1}$ to the graph, an appropriate reasonable initial value for $x_{k+1}$ is necessary. This value can be derived, for instance, from the prediction $h\left({\widehat{x}}_k,{\widehat{\alpha }}_k,z_k\right)$, where ${\widehat{x}}_k$ and ${\widehat{\alpha }}_k$ represent the most recent estimates of the state $x_k$ and the measurement ${\alpha }_k$, respectively.

Similarly, the bias factor associated with the modeled IMU errors is expressed as

$$f_{k+1}^{Bias}\left({\alpha }_{k+1},{\alpha }_k\right)\doteq d\left({\alpha }_{k+1}-g\left({\alpha }_k\right)\right)$$

(5)

with ${\alpha }_{k+1}$ and ${\alpha }_k$ represented as variable nodes in the factor graph.

Due to the high frequency of inertial measurement data (generally greater than 200 Hz), the inertial system has a high calculation frequency, which means that the number of variable nodes and corresponding cost functions in the navigation system factor graph model increase sharply, leading to a sharp increase in the computational complexity of joint optimization. Instead, consecutive IMU measurements can be combined into an equivalent IMU factor [29], relating between two distant navigation and calibration nodes, thereby avoiding introducing new variables into the optimization at IMU rate. The equivalent IMU factor is given by

$$f^{Equiv}\left(x_j,x_i,{\alpha }_k\right)\doteq d\left(x_j-h^{Equiv}\left(x_i,{\alpha }_i,\Delta x_{i\to j}\right)\right)$$

(6)

where $\Delta x_{i\to j}$ relates between the navigation states at the two time instances $x_i$ and $x_j$, comprising the accumulated change in position, velocity, and attitude. $h^{Equiv}$ represents the nonlinear function that predicts $x_j$ given $x_i$, ${\alpha }_i$ and $\Delta x_{i\to j}$.

The factor $f^{Equiv}$ encapsulates all IMU readings spanning the temporal interval between $t_i$ and $t_j$. However, integrating it into the factor graph entails introducing solely the variable $x_i$. This stands in juxtaposition to the conventional IMU factor scenario, where variables are added at the IMU sampling rate. Figure 2 shows the comparison between using the conventional IMU factor and using the equivalent IMU factor. The shown factor graph contains prior, IMU, equivalent IMU, and bias factors.

From Equations (3), (5), and (6), the equivalent IMU factor node $f_{k+1}^{Equiv}\left(x_{k+1},x_k,{\alpha }_k\right)$ is connected to the state variable node $x_k$, bias variable node ${\alpha }_k$, and state variable node $x_{k+1}$; The bias factor node $f_{k+1}^{Bias}\left({\alpha }_k,{\alpha }_{k+1}\right)$ connects the bias variable node ${\alpha }_k$ and the bias variable node ${\alpha }_{k+1}$. Figure 3a,b shows the factor graph models at two distinct time intervals. New nodes are continually integrated into the model as time progresses; therefore, the INS factor graph model is actually a dynamic graph model.

2.1.2. Prior Factor

Prior information $p\left(x_0\right)$ signifies that the navigation system possesses a predetermined understanding of the initial variables prior $x_0$ to real-time navigation. For instance, INS can determine the initial state accuracy of the carrier through initial alignment. The available prior information $p\left(x_0\right)$ can be subjected to additional factorization, resulting in discrete priors pertaining to relevant variables. Each of these priors is encapsulated by an individual prior factor. Termed as a unary factor, the prior factor is expressed as

$$f^{Prior}\left(x_0\right)\doteq d\left(x_0\right)$$

(7)

In the case of a Gaussian distribution, prior information is typically specified through a mean ${\mu }_0$ and a covariance matrix ${\Sigma }_0$.

2.1.3. GPS Factor

The GPS measurement equation is

$$z_k^{GPS}=h^{GPS}\left(x_k\right)+n^{GPS}$$

(8)

where $n^{GPS}$ represents the measurement noise, and $h^{GPS}$ stands for the measurement function, which establishes the relationship between the measurement $z_k^{GPS}$ and the carrier’s position.

The GPS factor equation is a unary factor defined as

$$f^{GPS}\left(x_k\right)\doteq d\left(z^{GPS}-h^{GPS}\left(x_k\right)\right)$$

(9)

which is solely connected to the node at time $t_k$. The GPS factor $f^{GPS}$ can be integrated into the graph along with an equivalent IMU factor $f^{IMU}$.

Figure 3b illustrates examples of factor graphs encompassing GPS measurements and data from various sensors operating at different rates. Additionally, it presents prior factor nodes associated with navigation and bias factor nodes.

2.1.4. VO Factor

When the true location of the target, as estimated by the stereo vision system, remains unknown, the vision system is limited to acquiring solely the relative position between the carrier and the target at the current time. Direct recovery of the carrier’s positioning information is not possible. VO offers an indirect method to ascertain the relative relationship between the carrier’s position state at two consecutive moments. This is achieved by utilizing the target as an intermediary reference position to estimate the relative position concerning the same target at both adjacent moments. It is evident that the measurement equation established by the visual system is intricately connected to the temporal states during imaging. Despite the inability of this measurement equation to satisfy the prerequisites of the KF, it equivalently introduces a factor node into the system factor graph model within the factor-graph-based state estimation algorithm.

The VO measurement equation is given by

$$z^{VO}=h^{VO}\left(x_k,x_{k+1}\right)+n^{VO}$$

(10)

where $n^{VO}$ is the measurement noise, $h^{VO}\left(x_k,x_{k+1}\right)$ is the measurement function, $z^{VO}$ is the measurement, $x_k$ and $x_{k+1}$ represent the state of VO at the associated time, respectively. The VO factor equation is a binary factor defined as

$$f^{VO}\left(x_k,x_{k+1}\right)=d\left(z^{VO}-h^{VO}\left(x_k,x_{k+1}\right)\right)$$

(11)

Figure 3c illustrates the VO factor node added into the INS factor graph model.

2.1.5. Loop Factor

In general, VO encapsulates the relative relationship between the carrier’s positional state at consecutive time points. When the carrier retraces its path to a prior position over a defined duration, constraints can be defined for these temporal state variables. Techniques like loop closure detection [30,31,32] offer a means to establish these constraints. This process is analogous to introducing a closed-loop factor node into the factor graph.

The Loop measurement equation is given by

$$z_{ij}^{Loop}=h^{Loop}\left(x_i,x_j\right)+n^{Loop}$$

(12)

where $n^{Loop}$ represents the measurement noise, $h^{Loop}\left(x_i,x_j\right)$ denotes the measurement function, $z_{ij}^{Loop}$ signifies the measurement. The Loop factor equation is defined by a binary factor

$$f^{Loop}\left(x_i,x_j\right)=d\left(z_{ij}^{Loop}-h^{Loop}\left(x_i,x_j\right)\right)$$

(13)

Figure 3d shows a factor graph in a multi-sensor and asynchronous scenario. The factor graph includes Loop, VO, GPS, IMU, bias, and prior factors. The IMU factor is replaced by the equivalent IMU factor as shown in Figure 2.

2.2. Factor Graph Inference Algorithm for Integrated Navigation

The factor graph model converts the solution of the maximum posterior estimation for integrated navigation state variables into factor graph inference. The inference algorithm employed for the factor graph is the sum-product algorithm [33], widely applied in practical engineering for precise or approximate reasoning, with documented success [34]. In light of the imperative for robust real-time performance in navigation systems, incremental inference technology [35] is employed. This approach is driven by the continuous expansion of the integrated navigation factor graph model, evolving dynamically over time.

2.2.1. Factor Graph Algorithm

In the factor graph model, the set of variable nodes is represented as $X$; The set of factor nodes is represented as $F$; The set of all edges of connecting nodes is represented as $E$. Thus, the factor graph can be represented as

$$G=\left(X,F,E\right)$$

(14)

Based on the theory of factor graphs, the factor graphs $G$ describe the factorization of functions $f\left(X\right)$, which can be denoted as

$$f\left(X\right)={\displaystyle \prod _i{f}_i\left(X_i\right)}$$

(15)

where $X_i$ represents the set of variable nodes connected to factor nodes $f_i$, $X_i\subseteq X$.

The problem needs to be solved in the factor graph inference algorithm to find the optimal estimate value $\widehat{X}$ of $X$, so that the function $f\left(X\right)$ takes the maximum value. The Maximum A Posteriori (MAP) estimate is expressed as

$$\widehat{X}=\underset{X}{\mathrm{argmax}}\left({\displaystyle \prod _i{f}_i\left(X_i\right)}\right)$$

(16)

For the factors defined in Equations (5)–(7), (9), (11), and (13), assuming a Gaussian noise model, the cost function can usually be defined as $d\left(a\right)\triangleq \exp \left(-\frac{1}{2}{‖a‖}_{\Sigma }^2\right)$. Here, ${‖a‖}_{\Sigma }^2\doteq a^T{\Sigma }^{-1}a$ represents the squared of the Mahalanobis distance, $\Sigma$ represents the covariance matrix. Then, the factor $f_i$ assumes the following form

$$f_i\left(X_i\right)\propto \exp \left(\frac{1}{2}{‖a_i‖}_{{\Sigma }_i}^2\right)$$

(17)

Therefore, taking the maximum value of the above equation is equivalent to the least squares problem given by

$$\underset{X}{\mathrm{argmin}}\left(-\log f\left(X\right)\right)=\underset{X}{\mathrm{argmin}}\frac{1}{2}{\displaystyle \sum _i{‖a_i‖}_{{\Sigma }_i}^2}$$

(18)

The expression of factor nodes in factor graph models is usually nonlinear and can be linearized through Taylor expansion. When the initial estimation ${\widehat{X}}_k$ of the system state $X_k$ is given, the update increment $\Delta$ of state $X_k$ can be transformed into solving a standard least squares solution

$${\Delta }^{\ast }=\underset{\Delta }{\mathrm{argmin}}\left(-\log f\left(\Delta \right)\right)=\underset{\Delta }{\mathrm{argmin}}{‖J\left({\widehat{X}}_k\right)\Delta -b\left({\widehat{X}}_k\right)‖}^2$$

(19)

where $\Delta =\left\{\delta X_k\right\}$ represents the update increment for the current system variable $X_k$, $J\left({\widehat{X}}_k\right)$ and $b\left({\widehat{X}}_k\right)$ are the Jacobian matrices and residuals of the cost functions at $X_k$; then, the Gaussian Newton iteration method is used to calculate the optimal estimation value ${\widehat{X}}_k$.

When the GPS and VO measurement frequency are different from the system output frequency and there is a time delay, it is only necessary to define the GPS or VO measurement values at time $t_k$ as factor nodes and connect them to the INS factor graph model. If GPS measurements are missing, the corresponding GPS factor node is not connected to the INS factor graph model at time $t_k$. Figure 4b illustrates the INS/GPS/VO factor model, reflecting the lower frequencies of GPS and VO measurements compared to the system output frequency, and the occurrence of missing GPS measurements.

The process of constructing the factor graph model for the integrated navigation system described above reveals that the factor nodes corresponding to each sensor are independent and do not affect each other in the factor graph model. For instance, in the case of GPS effectiveness change, if GPS fails suddenly, we simply suppress the addition of GPS factor nodes and transition to an INS/VO factor graph model. Conversely, when GPS becomes effective, adding GPS factor nodes reinstates the INS/GPS/VO factor graph model. The use of factor graph models to describe integrated navigation problems can flexibly handle dynamic changes in sensor effectiveness, and the integrated navigation algorithm based on factor graphs has strong scalability and flexibility.

2.2.2. IS Algorithm

The factor graph model intuitively manifests as the increasing number of variables in $X$ over time. It is clearly unable to meet the real-time requirements of the system, if every factor graph inference involves re-solving the optimal estimate for all variables $X$. Taking the factor graph model shown in Figure 3a as an example, variable elimination [36] is adopted and all nodes are eliminated from the factor graph by the order of $x_0\to {\alpha }_0\to x_1\to {\alpha }_1$, as shown in Figure 4a. A Bayesian network is a directed acyclic graph that encodes the conditional densities of each variable. The Bayesian network is equivalent to the upper triangular matrix $R$ resulting from the Jacobian matrix factorization $J=QR$ [37], thereby representing the square root information matrix of the system. The corresponding Jacobian matrix $J_{t_1}$ and the upper triangular matrix $R_{t_1}$ at time $t_1$, as shown in Figure 3a and obtained through a QR factorization of $J_{t_1}=Q{R}_{t_1}$, take the following form

$$J_{t_1}=\begin{array}{c}\begin{array}{cccc}{x}_0& {\alpha }_0& x_1& {\alpha }_1\end{array}\\ \left[\begin{array}{l}\begin{array}{cccc}\times & & & \end{array}\\ \begin{array}{cccc}\times & \times & \times & \end{array}\\ \begin{array}{cccc}& \times & & \end{array}\\ \begin{array}{cccc}& \times & & \times \end{array}\\ \begin{array}{cccc}& & \times & \end{array}\end{array}\right]\end{array},\hspace{0.17em}{R}_{t_1}=\begin{array}{c}\begin{array}{cccc}{x}_0& {\alpha }_0& x_1& {\alpha }_1\end{array}\\ \left[\begin{array}{l}\begin{array}{cccc}\times & \times & \times & \end{array}\\ \begin{array}{cccc}& \times & \times & \times \end{array}\\ \begin{array}{cccc}& & \times & \times \end{array}\\ \begin{array}{cccc}& & & \times \end{array}\end{array}\right]\end{array}$$

(20)

where $\times$ represents non-zero elements in matrix entries $J_{t_1}$ and $R_{t_1}$. Each linearized factor is denoted by corresponding rows in the Jacobian matrix $J_{t_1}$ and the upper triangular matrix $R_{t_1}$ corresponds to the square root information matrix of the system.

As seen, part of the Bayesian network is changed when new nodes are added, assuming that the new measurement $w$ and residual $r$ are added at time $t_2$. Compared to the factor graph at time $t_1$, the new state variable node $x_2$ and IMU bias nodes ${\alpha }_2$ are added into the factor graph at time $t_2$. Additionally, GPS, bias, and equivalent IMU factors are also introduced. The new factor graph is shown in Figure 3b, with the new added factors $f_2^{Equiv}$ and $f_2^{GPS}$. The updated Jacobian matrix is $J_{t_2}=\left[\begin{array}{l}{J}_{t_1}\\ w\end{array}\right]$ and the corresponding residual is $b^{\ast }=\left[\begin{array}{l}b\\ r\end{array}\right]$; thus, the upper triangular matrix $R_{t_2}$ is given by

$$R_{t_2}=\left[\begin{array}{cc}{Q}^T& \\ & I\end{array}\right]\left[\begin{array}{c}{J}_{t_1}\\ w\end{array}\right]=\left[\begin{array}{c}{R}_{t_1}\\ w\end{array}\right],\hspace{0.17em}{d}^{*}=\left[\begin{array}{cc}{Q}^T& \\ & I\end{array}\right]\left[\begin{array}{c}b\\ r\end{array}\right]=\left[\begin{array}{c}d\\ r\end{array}\right]$$

(21)

where $d^{\ast }$ is the residual term of the upper triangular matrix $R_{t_2}$ after QR factorization of the Jacobian matrix $J_{t_2}$. The updated Jacobian matrix $J_{t_2}$ and the upper triangular matrix $R_{t_2}$ can be shown as

$$J_{t_2}=\begin{array}{c}\begin{array}{cccc}\begin{array}{cc}\begin{array}{cc}{x}_0& {\alpha }_0\end{array}& x_1\end{array}& {\alpha }_1& x_2& {\alpha }_2\end{array}\\ \left[\begin{array}{cc}\begin{array}{cc}\begin{array}{cc}\begin{array}{cc}\begin{array}{cc}\begin{array}{l}\times \\ \times \\ \\ \\ \\ \\ \\ \end{array}& \begin{array}{l}\\ \times \\ \times \\ \times \\ \\ \\ \\ \end{array}\end{array}& \begin{array}{l}\\ \times \\ \\ \\ \times \\ \overline{\times }\\ \\ \end{array}\end{array}& \begin{array}{l}\\ \\ \\ \times \\ \\ \overline{\times }\\ \overline{\times }\\ \end{array}\end{array}& \begin{array}{l}\\ \\ \\ \\ \\ \overline{\times }\\ \\ \overline{\times }\end{array}\end{array}& \begin{array}{l}\\ \\ \\ \\ \\ \\ \overline{\times }\\ \end{array}\end{array}\right]\end{array},\hspace{0.17em}{R}_{t_2}=\begin{array}{c}\begin{array}{cccc}\begin{array}{cc}\begin{array}{cc}{x}_0& {\alpha }_0\end{array}& x_1\end{array}& {\alpha }_1& x_2& {\alpha }_2\end{array}\\ \left[\begin{array}{cc}\begin{array}{cc}\begin{array}{cc}\begin{array}{cc}\begin{array}{cc}\begin{array}{l}\times \\ \\ \\ \\ \\ \end{array}& \begin{array}{l}\times \\ \times \\ \\ \\ \\ \end{array}\end{array}& \begin{array}{l}\times \\ \times \\ \overline{\times }\\ \\ \\ \end{array}\end{array}& \begin{array}{l}\\ \times \\ \overline{\times }\\ \overline{\times }\\ \\ \end{array}\end{array}& \begin{array}{l}\\ \\ \overline{\times }\\ \overline{\times }\\ \overline{\times }\\ \end{array}\end{array}& \begin{array}{l}\\ \\ \\ \overline{\times }\\ \overline{\times }\\ \overline{\times }\end{array}\end{array}\right]\end{array}$$

(22)

where the new or changed non-zero variables are denoted as $\overline{\times }$. Obviously, two new variables $x_2$ and ${\alpha }_2$ are added into the factor graph and the corresponding Jacobian matrix has two new rows. Compared to the matrix $R_{t_1}$, only part of the variables are changed so that it is not necessary to recalculate the whole matrix $R_{t_2}$. The corresponding Bayes net is described in Figure 5b, and the changed variables are shown in red.

From Equations (20)–(22), non-zero terms emerge below the diagonal of $R_{t_1}$ when incorporating new measurements into the current measurement Jacobian matrix $J_{t_1}$ and upper triangular matrix $R_{t_1}$. Exploring the prospect of localized QR decomposition on the upper triangular matrix segments with non-zero terms below the diagonal yields a fresh upper triangular matrix $R_{t_2}$. Analogously, the residual terms $d^{*}$ are derived through corresponding operations. Figure 6 shows a schematic diagram of the incremental matrix $R_{t_2}$ calculation.

Owing to the inherent sparsity of the upper triangular matrix $R$ and the typical scenario where newly acquired measurement rows predominantly pertain to a limited set of variables, the region for local QR decomposition remains consistently compact and fixed with each iteration. Consequently, the majority of elements in the matrix $R$ remain unaltered pre and post-conversion. The incremental smoothing algorithm, grounded in batch optimization principles, maximizes the leverage of the sparsity in the system’s measurement Jacobian matrix $J$. It achieves this by progressively solving the matrix $R$, obviating the necessity for repeated decomposition of the measurement Jacobian matrix $J$ during each batch optimization cycle. Consequently, the incremental smoothing algorithm effectively curtails the computational complexity of the system, ensuring precision in solutions and bolstering real-time performance.

2.2.3. AFIS Algorithm

From the analysis of incremental QR decomposition in Equation (21), non-zero terms emerge beneath the diagonal of the associated upper triangular matrix $R$ upon the incorporation of new measurements into the integrated navigation system, necessitating local QR decomposition. However, the recently introduced measurement solely pertains to a subset of variables, influencing only the corresponding section of these variables in the upper triangular matrix (as shown in the red segment of the matrix $R_{t_2}$ in Figure 5). Consequently, continuous trajectory smoothing is deemed unnecessary, suggesting a more judicious approach of optimizing the impacted variables directly during the resolution of the joint optimization equation (smooth when the system has requirement). This optimization retains the integrity of the system’s matrix $R$. The matrix R of the entire system proves instrumental in executing global optimization, especially with the introduction of a loop factor in the system (optimizes variables within the closed loop) or when a navigation task requires comprehensive optimization of all navigation variables at a specific juncture (optimizes all navigation variables). Treating the matrix $R$ corresponding to the entire system as a network enables a swift execution of the optimization algorithm, employing a blend of adaptive local and global optimization. This amalgamation is aptly named the Adaptive Fast Incremental Smoothing (AFIS) algorithm, elucidated in the schematic diagram presented in Figure 7.

At each moment, when new sensor measurements are added to the integrated navigation system, corresponding factor nodes and variable nodes need to be added to the factor graph model, which is equivalent to adding new rows to the linearized least squares equations of the system. Due to the fact that newly added sensors only affect their adjacent state variables, local QR decomposition can be used to update the changing parts of the square root mean information matrix R in the system. The state variables that generate changes can form an adaptive window, and only the state variables X’ within that window are optimized and solved at each moment. Global state variables optimization is performed according to the system requirements. The system requirements here can be manually set, such as conducting global optimization every certain time or completing a certain maneuver task, depending on the computational requirements for algorithm complexity. The AFIS program flow chart is shown in Figure 8.

It can be seen that the AFIS algorithm is based on the foundations of the IS algorithm, focusing on extracting the variable regions directly influenced by measurements for subsequent local optimization. Clearly, the AFIS algorithm yields a notable reduction in the computational complexity of optimization solutions, thereby enhancing the real-time performance of the system. This algorithm not only exhibits adaptability in selectively choosing and optimizing the network based on the added type of measurement but also accommodates closed-loop factors, enabling it to undertake global optimization in alignment with navigation requirements.

3. Results

The simulation analysis is executed in the MATLAB 2016b environment on a desktop computer with an Inter^® Core i5 3.3 GHz CPU (Intel, Santa Clara, CA, USA), with specific simulation conditions outlined as follows:

(1)

Scenario 1: The simulation commences from a starting position with latitude ${34.246}^{\circ }$, longitude ${108.91}^{\circ }$, and altitude set at 380 m. The initial speed is 0 m/s, heading north. Following a sequence of actions, including acceleration, turning, and deceleration, the trajectory reaches a destination specified by latitude ${34.244}^{\circ }$, longitude ${108.9}^{\circ }$, and altitude of 380 m. The maximum speed during the trajectory is 20 m/s, and the total flight time is 250 s;

(2)

Scenario 2: The simulation begins at a starting position defined by latitude ${34.246}^{\circ }$, longitude ${108.91}^{\circ }$, and altitude of 380 m, with an initial speed of 0 m/s and heading north. Executing actions such as acceleration, turning, and deceleration, the trajectory reaches the destination defined by latitude ${34.244}^{\circ }$, longitude ${108.9}^{\circ }$, and altitude of 380 m, then returns to the initial position. The maximum speed during this trajectory is 20 m/s, and the total flight time is 400 s;

(3)

The IMU initialization data can be acquired through the application of inertial navigation’s differential equation based on the designed trajectory information. The constant drift and random drift of the gyroscope in INS is $1^{\circ }/\mathrm{h}$ and $0{.1}^{\circ }/\mathrm{h}$. The constant bias and random bias of the accelerometer is 100 μg and 10 μg;

(4)

GPS measurement noise is characterized by a mean square error of 2 m for longitude and latitude, 5 m for height, and 0.2 m/s for speed in all three directions;

(5)

The initial position of VO is the same as that of the carrier, and the VO position measurement noise is quantified by a mean square error of 1 m;

(6)

The mean square error of the measurement noise for the Loop factor is 2 m, and the measurement equation is shown in Section 2.1.5;

(7)

INS sampling frequency is set at 200 Hz, the GPS update frequency at 1 Hz, and the VO output frequency at 1 Hz.

3.1. Simulation of Sensor Effectiveness with Dynamic Changes

This section focuses on integrated INS/GPS navigation within Scenario 1, assuming a synchronous output frequency between INS and GPS, with the GPS measurements being unavailable during the period from 90 s to 160 s. The objective is to compare the fusion accuracy of the Kalman filtering (KF) and factor-graph-based (FG) method without time delay in measurement. The evaluation specifically examines the factor graph algorithm’s capacity to accommodate dynamic changes in sensor effectiveness. Figure 9 shows the real-time position error of both KF and FG.

In this experiment, GPS measurements were not available from 90 to 160 s, and the efficacy of sensors in the integrated navigation system underwent two changes. The integrated navigation system transitions from INS/GPS mode to INS individual mode when the GPS measurements are unavailable at 90 s. The inclusion of factor node $f^{GPS}$ is inhibited in the factor graph model, resulting in a rapid accumulation of position errors due to the lack of INS correction. Subsequently, as the GPS signal becomes effective again at 160 s, the integrated navigation system shifts from INS individual mode to INS/GPS mode, and the factor node $f^{GPS}$ is reintroduced to the factor graph model, correcting the position error.

Upon analysis, it is evident that during transitions between INS and INS/GPS modes prompted by changes in sensor effectiveness, only factor nodes associated with invalidated sensors need to be suppressed or new factor nodes related to effective sensors need to be added to the factor graph model. No additional adjustments are required, ensuring a seamless switch between different modes of the integrated navigation system and enhancing its overall robustness.

3.2. Simulation of Sensor Asynchrony and Delay

Under simulation scenario 1, assuming stable and effective measurement information from all sensors within the INS/GPS integrated navigation system, we established various asynchronous and delay conditions between the GPS measurement frequency and the INS output frequency. This aimed to assess the factor graph algorithm’s capability in managing sensor asynchrony and delay. The term “asynchronous” denotes the discrepancy in output frequencies between INS and GPS. We assumed a time delay in GPS measurements and configured delay times as dt = 0.1 s, dt = 0.3 s, and dt = 0.5 s, respectively. We subsequently compared these results with the integrated navigation of FG and KF without delay (dt = 0 s). The simulation results are shown in Figure 10 and

Table 1. RMSE statistical result of KF and FG in different measurement delays.

Integrated Navigation Method	Wihout Delay		dt = 0.1 s	dt = 0.3 s	dt = 0.5 s
	KF	FG	FG	FG	FG
Radial Error (m)	18.32	14.27	14.57	15.17	15.81

.

From Figure 10 and Table 1, it is evident that the positional error increases to a certain extent in the presence of asynchrony and delay. This occurs primarily because the system output frequency surpasses the GPS measurement frequency, compelling reliance solely on INS without GPS measurements. Notably, the FG algorithm exhibits a lesser sensitivity to delay times, sustaining comparable accuracy to the KF algorithm even in the absence of delay measurements. The FG algorithm requires no adjustment for sensor asynchrony and delay. Varied delay measurements merely alter the sequencing of corresponding cost functions in deriving global cost functions, without affecting the optimization results. Instead, sensor measurement values received must be systematically denoted as factor nodes, progressively connected to corresponding variable nodes in the factor graph. Subsequently, the incremental inference algorithm is employed to deduce the factor graph, yielding the optimal estimate of the navigation state.

3.3. AFIS Algorithm Analysis

The section primarily examines the accuracy and real-time performance of the adaptive fast incremental smoothing factor-graph-based (AFIS-FG) algorithm, batch optimization factor-graph-based (Batch-FG) algorithm, fixed-lag smoothing factor-graph-based (FixLag-FG) algorithm, and incremental smoothing (IS-FG) algorithm. The IS-FG algorithm and our proposed AFIS-FG algorithm are shown in Section 2.2.2 and Section 2.2.3, respectively. The Batch-FG algorithm recalculates all state variables of the nonlinear least squares problem, which is shown in references [20,21,22]. The FixLag-FG algorithm [23] maintains several past states in a sliding window, with all variables within the sliding window undergoing recalculation upon obtaining a new measurement, while FixLag-FG (5) means the length of the sliding window is 5. The simulation aims at the INS/GPS/VO integrated navigation in scenario 1 and scenario 2. The simulation results are shown in Figure 11 and Figure 12,

Table 2. RMSE statistical result of different algorithms in two different scenarios.

Optimization Algorithm		Batch-FG	IS-FG	AFIS-FG	FixLag-FG (5)
Radial Error (m)	Scenario 1	0.907	1.308	0.940	1.942
	Scenario 2	1.235	1.570	1.554	2.588

and

Table 3. Total runtime statistical result of different algorithms in two different scenarios.

Optimization Algorithm		Batch-FG	IS-FG	AFIS-FG	FixLag-FG (5)
Time (s)	Scenario 1	54.6	7.4	4.7	0.2665
	Scenario 2	257.1	32.5	20.9	0.3971

.

From Figure 12 and Table 3, it is evident that FixLag (5) exhibits the shortest optimization time, clocking in at 0.2665 s in scenario 1 and 0. 3971 s in scenario 2. This efficiency arises from its exclusive focus on optimizing and updating local state variables within the smoothing window at each moment. In contrast, the optimization times of Batch-FG, IS-FG, and AFIS-FG gradually escalate as simulation time progresses. Batch-FG stands out with the lengthiest optimization time, recorded at 54.6 in scenario 1 and 257.1 s in scenario 2. This is attributed to Batch-FG’s overarching approach, involving unified processing of all received measurements at all times. This approach is akin to batch processing, entailing the update of the system’s upper triangular matrix $R$ at each time, followed by global optimization.

IS-FG secures the second shortest optimization time, totaling 7.4 s in scenario 1 and 32.5 s in scenario 2. This is a consequence of IS-FG’s incremental update of the upper triangular matrix $R$ of the system at each moment, a departure from the Batch-FG’s approach. AFIS-FG follows with the third shortest optimization time, amounting to 4.9 s in scenario 1 and 20.9 s in scenario 2. AFIS-FG employs incremental updates to the upper triangular matrix of the system upon receiving measurements, focusing its optimization on locally affected state variables. Additionally, global optimization is executed by retaining the upper triangular matrix in the presence of a loop factor in the factor graph.

The precision analysis from Figure 11 and Table 2 reveals that the accuracy achieved by IS-FG and AFIS-FG is comparable to that of Batch-FG, surpassing the performance of FixLag-FG (5). This discrepancy arises due to the distinct optimization strategies employed by these algorithms. Specifically, Batch-FG, IS-FG, and AFIS-FG optimize global variables, whereas FixLag-FG (5) exclusively focuses on variables within a smoothing window. Notably, during instances of a loop factor in the system (as evident in the 310 s segment in Figure 11b), Batch-FG, IS-FG, and AFIS-FG leverage this loop factor to rectify the current time state, thereby enhancing real-time navigation accuracy. In contrast, FixLag-FG (5) lacks this capability.

4. Discussion

In summary, this paper proposes a factor-graph-based INS/GPS/VO integrated navigation method. Compared with traditional filtering-based (such as KF) integrated navigation methods, the factor-graph-based integrated navigation method can achieve plug-and-play functionality when facing asynchronous and delayed sensor measurements, and can handle multi-state correlated measurements such as VO, while also having high navigation accuracy. Section 3.1 and Section 3.2 also verified this point. Therefore, when dealing with multi-state correlated measurements and facing asynchronous and delayed sensor measurements, using factor-graph-based integrated navigation methods has better accuracy and flexibility, and can achieve plug-and-play functionality. Although its computational complexity is higher than that of filter-based combined navigation methods, this aspect can optimize the algorithm to reduce its computational complexity. Furthermore, we also propose an adaptive fast incremental smoothing (AFIS) algorithm. Compared with existing factor graph optimization algorithms based on incremental smoothing (IS), the AFIS algorithm proposed in this paper can greatly improve the speed of factor graph optimization algorithms while maintaining comparable accuracy, which can better meet the real-time requirements of factor-graph-based integrated navigation methods. Simultaneously, the algorithm can be divided into two components: global optimization and local optimization. Local optimization pertains to optimizing newly added measurement-related partial state variables at each moment, whereas global optimization involves optimizing state variables continuously, which can be performed at regular intervals according to system requirements. Although the accuracy of global optimization at any given moment may not match that of traditional incremental smoothing methods, it is evident that this optimization mode offers greater flexibility. Section 3.3 also verified that the AFIS algorithm notably enhances computational speed while maintaining comparable accuracy to the IS algorithm.

5. Conclusions

This paper introduces an adaptive fast integrated navigation algorithm for INS/GPS/VO, employing a factor graph framework. The study assesses the algorithm’s efficacy in managing dynamic changes in sensor effectiveness, handling sensor measurements with asynchronous and delayed presence, and optimizing the factor graph. The simulation results demonstrate that when the effectiveness of the sensor changes, it only needs to suppress or add its factor nodes into the factor graph model to achieve seamless switching of the integrated navigation system working mode, which makes the system highly robust. For asynchronous and delayed sensors, the factor graph algorithm directly defines received sensor measurements as factor nodes linked to the respective variable nodes in the factor graph. This approach eliminates the need to synchronize measurements from different sensors to the same time, thereby mitigating the introduction of additional human error. In comparison to traditional incremental smoothing factor graph optimization algorithms, the AFIS algorithm proposed in this paper significantly enhances optimization speed while maintaining comparable accuracy. The excellent scalability and flexibility of the factor graph model confers substantial advantages in addressing multi-sensor integrated navigation challenges. Consequently, the INS/GPS/VO integrated navigation algorithm, based on the factor graph, holds considerable reference value for diverse multi-sensor integrated navigation systems in various application environments.