# A Robust Localization System Fusion Vision-CNN
## 4 Simulation
### Dynamic Pose Tracking of Robot  
This section is mainly to verify the high-precision pose tracking performance of the algorithm. The positioning system involves three main coordinate systems: map, odom, and base. The base coordinate system is the coordinate system bound to the robot, the EKF filter [41] is estimated in the odom coordinate system, and the global pose is defined in the map. Before particle sampling, we use the EKF framework to fuse the robot inner motion sensor's IMU and the wheel odometer information to obtain the predicted initial pose of the robot in the odom coordinate system. Assume that the fused poses at time \(k-1\) and \(k\) are \((x_{k-1}^{odom\_base},y_{k-1}^{odom\_base},\phi_{k-1}^{odom\_base})\) and \((x_{k}^{odom\_base},y_{k}^{odom\_base},\phi_{k}^{odom\_base})\), respectively. Then, the global pose of the robot at time \(k\) is:  
\[\left\{ \begin{aligned} x_{k}^{map\_base}&=x_{k-1}^{ map\_base}+\delta_{trans}\cos(\varphi_{k-1}^{map\_base}+\delta_{rot1})\\ y_{k}^{map\_base}&=y_{k-1}^{map\_base}+\delta_{ trans}\sin(\varphi_{k-1}^{map\_base}+\delta_{rot1})\\ \varphi_{k}^{map\_base}&=\varphi_{k-1}^{map\_base }+\delta_{rot1}+\delta_{rot2}\end{aligned} \right. \tag{16}\]  
Figure 10: Search time from coarse to fine pose. where:  
\[\left\{ \begin{array}{l}\delta_{rot1}=\text{arctan2}(y_{k}^{adom\_base}-y_{k-1}^{ adom\_base},x_{k}^{adom\_base}-x_{k-1}^{adom\_base})-\varphi_{k-1}^{adom\_base} \\ \delta_{trans}=\sqrt{\left(x_{k}^{adom\_base}-x_{k-1}^{adom\_base}\right)^{2}+ \left(y_{k}^{adom\_base}-y_{k-1}^{adom\_base}\right)^{2}}\\ \delta_{rot2}=\varphi_{k}^{adom\_base}-\varphi_{k-1}^{adom\_base}-\delta_{rot1} \end{array} \right. \tag{17}\]  
Let the robot follow basically the same trajectory as in Section 4.2 and compare the pose tracking accuracy of EKF, AMCL, and the algorithm proposed in this paper. Carry out the two experiments of case 1 and case 2, respectively, where case 1 is the IMU and the wheel odometer has a small error, and Case 2 is the IMU and the wheel odometer has a large error. In the case of case 1, the results of pose estimation and the corresponding pose errors of EKF, AMCL, and the algorithm proposed in this paper are shown in Figure 11. The statistical results of the error are shown in Table 2. Taking RMSE statistical results as an example, compared with the AMCL algorithm, the proposed algorithm has improved pose tracking accuracy by 9.75%, 14.98%, and 8.6% in the \(x\), \(y\), and \(yaw\) directions, respectively. Both AMCL and the algorithm proposed in this paper can achieve a higher accuracy, with the RMSE value of the position error being less than 0.05 m (the size of a grid). Figure 11: The pose estimation result of case 1. (**a**) The estimated error in \(x\) direction; (**b**) The estimated error in \(y\) direction; (**c**) The estimated error in the \(yaw\) direction; (**d**) True trajectory and estimated trajectory. In case 2, the pose estimation and the pose errors of EKF, AMCL, and the algorithm proposed in this paper are shown in Figure 12. It can be seen from Figure 11 and Table 2 that when the error of the wheel odometer and the IMU is large, the correlation sampling algorithm proposed in this paper and AMCL algorithm can also achieve high accuracy, which is the same as the conclusion of case 1. At the same time, we can also find that when the IMU and the wheel odometer errors are large, the accuracy of the pose tracking based on the AMCL algorithm will be slightly increased, but the method proposed in this article has basically no effect. In practical applications, the error of the IMU and the wheel odometer is inevitable, and the method proposed in this paper can still keep the accuracy unaffected or less affected, which is one of the major advantages of the algorithm proposed in this paper. From the above experiment, we can see that the accuracy of the algorithm in this article and the AMCL algorithm are slightly improved, but the difference is not big.