## 3 Factor Graph Integrated Navigation Framework
### IMU Pre-Integration Factor Node
When describing the carrier's attitude using quaternions, the update of the carrier's attitude between two adjacent IMU sampling times can be expressed as:  
\[q_{b(k)}^{n}=q_{b(k-1)}^{n}\otimes q_{b(k-1)}^{n(k-1)}\otimes q_{b(k)}^{n(k-1)} \tag{11}\]  
Updating the carrier's attitude information using pre-integration for quaternions from \(t_{k-1}\) to \(t_{k}\) can be expressed as:  
\[q_{b(k)}^{b(k-1)}=\left(\left(q_{b(k)}^{n(k)}\right)^{-1}\otimes q_{n(k-1)}^{n( k)}\otimes q_{b(k-1)}^{n(k-1)}\right)^{-1} \tag{12}\]  
The velocity update equation for the carrier can be expressed as:  
\[v_{b(k)}^{n}=v_{b(k-1)}^{n}+\int_{t_{k-1}}^{t_{k}}C_{b(t)}^{n(t)}(\tilde{f}^{b }-b_{a}-\varepsilon_{a})dt+\int_{t_{k-1}}^{t_{k}}\left(g^{n}-(2\omega_{ie}^{n} +\omega_{en}^{n})\times v^{n}\right)dt \tag{13}\]  
where \(g^{n}\) is the local gravity. The velocity pre-integration from \(t_{k-1}\) to \(t_{k}\) can be expressed as:  
\[ \begin{array}{l}\triangle v_{b(k)}^{b(k-1)}=C_{n(t)}^{b(k-1)}(v_{b(k)}^{n}- v_{b(k-1)}^{n}-g^{n}\triangle\,t_{k-1,k}+\left((2\omega_{ie}^{n}+\omega_{en}^{n} \right)\times v_{b(k-1)}^{n}\triangle\,t_{k-1,k})\\ =\int_{t_{k-1}}^{t_{k}}C_{b(t)}^{b(k-1)}(\tilde{f}^{b}-b_{a}-\varepsilon_{a}) dt\end{array}  \tag{14}\]  
Integrating the velocity of the carrier yields the position increment of the carrier, which, when added to the previous position, provides the current pose measurement. Therefore, the position update equation for the carrier can be expressed as:  
\[p_{b(k)}^{n}=p_{b(k-1)}^{n}+\int_{t_{k-1}}^{t_{k}}v_{b(t)}^{n}dt \tag{15}\]  
The position pre-integration from \(t_{k-1}\) to \(t_{k}\) can be expressed as:  
\[ \begin{array}{l}\triangle P_{b(k)}^{b(k-1)}=C_{n(t)}^{b(k-1)}(p_{b(k)}^{n}- p_{b(k-1)}^{n}-v_{b(k-1)}^{n}\triangle\,t_{k-1,k}\\ +0.5((2\omega_{ie}^{n}+\omega_{en}^{n})\times v_{b(k-1)}^{n}-g^{n})\triangle \,t_{k-1,k}^{2})\\ =\int\int_{t_{k-1}}^{t_{k}}C_{b(t)}^{b(k-1)}(\tilde{f}^{b}-b_{a}-\varepsilon_{ a})dt^{2}\end{array}  \tag{16}\]  
During the position update process using the position pre-integration model, the influences of \(\omega_{ie}^{n}\) and \(\omega_{en}^{n}\) are considered, which improves the position solving accuracy compared to traditional inertial pre-integration methods. In factor graph optimization, a noise covariance matrix is needed to weight the IMU factors, and the error state vector is defined as follows:  
\[\delta z_{t}=\left[\delta p_{b(k)}^{b(k-1)}\quad\delta v_{b(k)}^{b(k-1)}\quad \delta q_{b(k)}^{b(k-1)}\quad\delta b_{\omega}\quad\delta b_{a}\right]^{T} \tag{17}\]In the formula, \(\delta p_{b(k)}^{b(k-1)},\delta v_{b(k)}^{b(k-1)},\delta q_{b(k)}^{b(k-1)}\) represents the pre-integrated measurement errors for position, velocity, and attitude, while \(\delta b_{\omega},\delta b_{a}\) represents the bias errors for the gyroscope and accelerometer. The continuous-time system's error state equation is expressed as:  
\[\delta\dot{z}_{t}=F_{t}\delta z_{t}+G_{t}\varepsilon_{t} \tag{18}\]  
where the system error state transition matrix \(F_{t}\), the process noise transition matrix \(G_{t}\), and the noise vector \(\varepsilon_{t}\) are represented as:  
\[F_{t}= \begin{bmatrix}0&I&0&0&0\\ 0&0&-\mathbb{C}_{p(t)}^{b(k-1)}(f^{b}-b_{a})\times&0&\mathcal{C}_{b(t)}^{b(k-1 )}\\ 0&0&-(\hat{\alpha}_{b}^{i}-b_{\omega})\times&-I&0\\ 0&0&0&0&0\\ 0&0&0&0&0\end{bmatrix}  \tag{19}\]  
\[G_{t}= \begin{bmatrix}0&0\\ 0&\mathcal{C}_{p(t)}^{b(k-1)}\\ -I&0\\ 0&0\\ 0&0\end{bmatrix} ,\varepsilon_{t}= \begin{bmatrix}\varepsilon_{\omega}\\ \varepsilon_{a}\end{bmatrix}  \tag{20}\]  
The pre-integration covariance matrix can be expressed as:  
\[\sum_{b(t)}^{imu}=\Phi_{t}\sum_{b(t-1)}^{imu}\Phi_{t}{}^{\text{T}}+\int_{t-1} ^{t}\Phi_{t}G_{t}Q{G_{t}}^{\text{T}}\Phi_{t}{}^{\text{T}}dt \tag{21}\]  
where \(\sum_{b(t-1)}^{imu}=0\) is the initial covariance matrix, \(\Phi_{t}\approx I+F_{t}\) is the transition matrix, and \(Q=diag[\sigma_{\omega}^{2},\sigma_{a}^{2}]\) is the discrete-time noise covariance matrix.