## 4 Methodology
### Fusion  
The EKF algorithm is employed for the fusion process. EKF is an enhancement of the traditional Kalman filter, with its core resembling the original Kalman filter, consisting of two main processes: time update and measurement update. Time update predicts the state variables and calculates the prior estimate of the covariance; the measurement update feeds back the estimation errors to the loop for state correction. The state matrix is modeled using position \(\delta P(\delta Px,\delta Py,\delta Pz)\), velocity \(\delta V(\delta Vx,\delta Vy,\delta Vz)\), attitude \(\delta A\) (\(\delta\theta,\delta\varphi,\delta\varphi\)), accelerometer bias errors \(\delta\mathbf{\alpha}\) (\(\delta a_{x},\delta a_{y},\delta a_{z}\)), and gyroscope bias errors \(\delta\mathbf{\omega}\) (\(\delta\omega_{x},\delta\omega_{y},\delta\omega_{z}\)). \[\mathbf{X}= \begin{bmatrix}\delta\mathbf{P}&\delta\mathbf{V}&\delta\mathbf{A}&\delta\mathbf{a}& \delta\mathbf{\omega}\end{bmatrix} ^{T} \tag{18}\]  
The system state equation is given by:  
\[\mathbf{X}_{k+1}=\mathbf{F}\mathbf{X}_{k}+\mathbf{G}\mathbf{W}_{k} \tag{19}\]  
\[\mathbf{F}= \begin{bmatrix}\mathbf{I}_{3\times 3}&\Delta t\bullet\mathbf{I}_{3\times 3}& \mathbf{0}&\mathbf{0}&\mathbf{0}\\ \mathbf{0}&\mathbf{I}_{3\times 3}&\mathbf{S}\mathbf{t}\bullet\Delta t&\mathbf{C}_{\mathbf{0}}^{\text{n}} \bullet\Delta t&\mathbf{0}\\ \mathbf{0}&\mathbf{0}&\mathbf{I}_{3\times 3}&\mathbf{0}&-\mathbf{C}_{\mathbf{0}}^{\text{n}}\bullet\Delta t \\ \mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{B}_{1}\bullet\mathbf{I}_{3\times 3}&\mathbf{0}\\ \mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{B}_{2}\bullet\mathbf{I}_{3\times 3}\end{bmatrix}  \tag{20}\]  
\[\mathbf{S}\mathbf{t}= \begin{bmatrix}0&-a_{z}&a_{y}\\ a_{z}&0&-a_{x}\\ -a_{y}&a_{x}&0\end{bmatrix}  \tag{21}\]  
where \(\mathbf{X}_{k+1}\) denotes the system state at time \(k+1\); \(\mathbf{F}\) is the Jacobian matrix of the motion model with respect to the error and \(\mathbf{G}\) is the Jacobian matrix with respect to the noise; \(\mathbf{W}_{k}\) represents zero-mean Gaussian white noise with covariance \(\mathbf{Q}_{k}\); \(B_{1}\) and \(B_{2}\) denote the  
Figure 13: Visualization of feature matching results without step length constraint and with step length constraint in environments with repeating objects. measurement white noise of the accelerometer and gyroscope, respectively; \(\mathbf{I}\) and \(\mathbf{0}\) are the identity matrix and zero matrix, respectively. The system measurement Equation is given by:  
\[\mathbf{Z}_{k}=\mathbf{H}\mathbf{X}_{k}+\mathbf{V}_{k} \tag{22}\]  
\[\mathbf{Z}= \begin{bmatrix}P_{\text{IMU}x}-P_{\text{cam}x}\\ P_{\text{IMU}y}-P_{\text{cam}y}\\ P_{\text{IMU}z}-P_{\text{cam}z}\\ \theta_{\text{IMU}}-\theta_{\text{cam}}\\ \phi_{\text{IMU}}-\phi_{\text{cam}}\\ \phi_{\text{IMU}}-\phi_{\text{cam}}\\ \end{bmatrix}  \tag{23}\]  
where \(\mathbf{Z}\) represents the measurement vector. \(P_{\text{IMU}x}\), \(P_{\text{IMU}y}\), \(P_{\text{IMU}z}\) denote the position information obtained by the inertial data; \(P_{\text{cam}x}\), \(P_{\text{cam}y}\), \(P_{\text{cam}z}\) denote the position information obtained by the camera data; \(\theta_{\text{IMU}}\), \(\varphi_{\text{IMU}}\), \(\varphi_{\text{IMU}}\) are the roll, pitch, and yaw angles obtained by the inertial data, respectively; and lastly, \(\theta_{\text{cam}}\), \(\varphi_{\text{cam}}\), \(\phi_{\text{cam}}\) are the roll, pitch, and yaw angles obtained by the inertial device data, respectively. \(\mathbf{H}\) denotes the measurement matrix; \(\mathbf{V}_{k+1}\) represents zero-mean Gaussian white noise with covariance \(\mathbf{R}_{k+1}\). The matrix \(\mathbf{H}\) can be expressed as  
\[\mathbf{H}= \begin{bmatrix}\mathbf{I}_{3\times 3}&\mathbf{0}_{3\times 6}&\mathbf{0}_{3\times 6} \\ \mathbf{0}_{3\times 3}&\mathbf{I}_{3\times 3}&\mathbf{0}_{3\times 9}\\ \end{bmatrix}