# Research on GNSS/IMU/Visual Fusion Positioning Based on Adaptive Filtering
## 2 Fusion Algorithm  
### Visual Odometry Algorithm  
ORB-SLAM2 is a real-time SLAM system based on monocular, stereo, and RGB-D cameras [16], developed by [PERSON] et al. [17] from the University of Zaragoza, Spain, for building three-dimensional maps and positioning the camera's location and orientation without GPS signals or with limited sensor information. ORB-SLAM2 takes each frame's image and timestamp, configuration file, and dictionary file as inputs, and outputs the camera poses corresponding to each frame, keyframes, and map points, with its framework consisting of three threads (tracking, local mapping, and loop closing) that interact through keyframes. Upon entering the program and after the map is automatically initialized, there may be two scenarios for estimating the camera's pose during tracking. The first scenario involves the known translational and rotational velocities of the previous frame. The camera pose is estimated using the Perspective-n-Point (PnP) [18] method, which optimizes the camera pose based on matched 3D spatial points and 2D feature points of the current frame, formulating a least-squares problem to solve for the optimal camera pose. By calculating the Jacobian [19] of the error concerning the pose, optimization algorithms such as the Gauss-Newton method [20] or the Levenberg-Marquardt method [21] can be used to iteratively solve this least-squares problem to obtain the camera pose that minimizes the projection error. For a spatial point \(P\) with homogeneous coordinates \(\mathrm{P}=[X,Y,Z,1]^{T}\), its projection coordinates in image \(I_{1}\) are \(p_{1}=[u_{1},v_{1}]^{T}\), its re-projection coordinates in image \(I_{2}\) are \(p^{\prime}{}_{2}=[u^{\prime}{}_{2},v^{\prime}{}_{2}]^{T}\), and its observed value in the image \(I_{2}\) is \(p_{2}=[u_{2},v_{2}]^{T}\). The re-projection error is \(e=p_{2}-p^{\prime}{}_{2}\), and the ideal re-projection process is expressed as follows:  
\[s_{2}u_{2}=Kexp(\xi^{\wedge})P \tag{1}\]  
where \(\mathrm{s}_{2}\) represents the depth of the spatial point \(P\) in the camera coordinate system of image \(I_{2}\); \(K\) represents the camera intrinsic parameters; \(exp(\xi^{\wedge})\) represents the pose transformation matrix of the camera from image \(I_{1}\) to image \(I_{2}\). However, due to the poor initial estimation of the camera pose, the re-projection usually has some error compared to the true value. This error is defined as follows:  
\[e_{2}=u_{2}-\frac{1}{s_{2}}Kexp(\xi^{\wedge})P \tag{2}\]  
Typically, multiple feature points are observed under a single camera pose. Assuming we have \(N\) feature points, this forms a least-squares problem for estimating the camera pose \(\xi\):  
\[\xi^{*}=\underset{\xi}{\arg\min}\frac{1}{2}\sum_{i=1}^{N}\|e_{i}\|_{2}^{2}= \underset{\xi}{\arg\min}\frac{1}{2}\sum_{i=1}^{N}\left\|u_{i}-\frac{1}{s_{i }}K\exp(\xi^{\wedge})P_{i}\right\|_{2}^{2} \tag{3}\]  
In order to solve this optimization problem, the first-order Taylor expansion of the objective function near the current pose is performed to obtain a linear approximation \(\mathrm{e}_{i}(x+\Delta x)\approx e_{i}+J\Delta x\). When the error term \(\mathrm{e}\) is a two-dimensional coordinate error and \(\mathrm{x}\) is the six-dimensional camera pose error, the Jacobian matrix \(\mathrm{J}\) will be a 2 \(\times\) 6 matrix. According to the Lie group and algebra, the reprojection error is multiplied by the perturbation momentum at the same time, and then the derivative of the perturbation with respect to the change of \(\mathrm{e}\) is considered, which can be obtained by using the chain rule:  
\[J=\frac{\partial e}{\partial\delta\xi}=\frac{\partial e}{\partial P^{\prime}} \frac{\partial P^{\prime}}{\partial\delta\xi} \tag{4}\]where \(\frac{\partial\epsilon}{\partial P^{\prime}}\) is the derivative of the error with respect to the projection point. \[\frac{\partial e}{\partial P^{\prime}}=-