# Research on GNSS/IMU/Visual Fusion Positioning Based on Adaptive Filtering
## 2 Fusion Algorithm
### Overall Design of GNSS/IMU/Visual Fusion Positioning  
By processing inertial navigation system mechanization to the specific force and angular velocity outputs from the IMU, the changes in the position, velocity, and attitude of the carrier can be obtained. This method provides accurate positioning in the short term and is not affected by external interference. Since the navigation information is generated through integration, the positioning error gradually increases over time. To reduce the positioning error and enable long-term positioning, adaptive filtering is used to fuse the outputs of the IMU and GNSS. GNSS can provide relatively accurate positioning information over the long term, but it is susceptible to factors such as high-rise building obstructions. Based on the physical characteristics of the IMU and satellite receiver, an adaptive filtering fusion approach can be designed. The difference between the GNSS measurement information and the predicted position and velocity from the INS is used as the innovation for measurement updates. This allows for the estimation and feedback correction of relevant parameters in the state equation, thereby achieving closed-loop compensation for the IMU parameters. The state equation can be formulated as follows:\[\dot{x}(t)=F(t)x(t)+G(t)w(t) \tag{7}\] \[x(t)=[(\delta r_{INS}^{n})^{T},(\delta v_{INS}^{n})^{T},\phi^{T},b_{g}^{ T},b_{a}^{T},s_{g}^{T},s_{a}^{T}]^{T} \tag{8}\]  
where: \(\delta r_{INS}^{n}\) represents the INS position error; \(\delta v_{INS}^{n}\) represents the INS velocity error; \(\phi\) represents the INS attitude error; \(b_{g}\) represents the gyroscope bias; \(b_{a}\) represents the accelerometer bias; \(s_{g}\) represents the gyroscope scale factor error; \(s_{a}\) represents the accelerometer scale factor error. The observation equation for the loosely coupled integration [23] can be derived from the GNSS positioning results as follows:  
\[Z(t)=\widetilde{r}_{INS}-\widetilde{r}_{GNSS}=H(t)x(t)+v(t) \tag{9}\]  
where: \(\widetilde{r}_{INS}\) represents the INS position output; \(\widetilde{r}_{GNSS}\) represents the GNSS position observation output; the measurement noise vector is zero-mean white noise [24], satisfying \(E[v_{k}]=0\) and \(E[v_{k}v_{j}^{T}]=R_{k}\delta_{kj}\). However, in practical applications, there are errors in obtaining the structural parameters and noise statistical parameters of the stochastic system. These errors can reduce the filtering accuracy and even cause filter divergence. Therefore, it is necessary to effectively estimate the measurement noise covariance matrix \(\hat{R}_{k}\). By reconstructing the measurement noise covariance matrix through weighted recursive estimation, it can be derived as follows:  
\[\hat{R}_{k}=(1-\beta_{k})\hat{R}_{k-1}+\beta(\widetilde{Z}_{k/k-1}\widetilde{ Z}_{k/k-1}^{T}-H_{k}P_{k/k-1}H_{k}^{T}) \tag{10}\]  
\[\beta_{k}=\frac{\beta_{k-1}}{\beta_{k-1}+b} \tag{11}\]  
where \(b\) is referred to as the fading factor, with smaller values indicating stronger adaptability, commonly set to \(b=0.9\sim 0.999\)[25]. However, it should be noted that the forgetting factor \(b\) cannot be set to too small a value, otherwise the noise estimation results will exhibit excessive fluctuations. GNSS and IMU data are combined using adaptive filtering to obtain a simplified loosely coupled navigation pose, which is then further fused with the pose data from visual odometry using adaptive filtering. Due to the difference in sampling frequencies, the pose data output from the integrated navigation system is at 1 Hz, while the pose data from visual odometry depends on keyframes and may not be at 1 Hz. To address the issue of pose timestamp misalignment caused by different sampling frequencies, the timestamps of the integrated navigation and visual outputs are first converted to GPS week numbers and seconds of the week, and then processed using interpolation [26] to achieve time synchronization.