# A Robust Localization System Fusion Vision-CNN
## 3 System Overview and Methodology
### Correlation Particle Sampling Method  
Brute force sampling according to the motion model is to sample the displacement and the angle at the predicted position at the same time according to the Gaussian distribution. Each particle state information contains the particle's pose and weight information and then scores according to each particle's pose state. Assuming that the particle pose state is \((x,y,\theta)\), the coordinates of the laser measurement point \((z_{k}^{t},\theta_{k,sens}^{t})\) in the global coordinate system are:  
\[\left[\begin{array}{c}x_{z_{k}^{t}}\\ y_{z_{k}^{t}}\end{array}\right]=\left[\begin{array}{c}x\\ y\end{array}\right]+\left[\begin{array}{cc}\cos\theta&-\sin\theta\\ \sin\theta&\cos\theta\end{array}\right]\left[\begin{array}{c}x_{k,sens}\\ y_{k,sens}\end{array}\right]+z_{k}^{t}\left[\begin{array}{c}\cos(\theta+ \theta_{k,sens}^{t})\\ \sin(\theta+\theta_{k,sens}^{t})\end{array}\right] \tag{6}\]  
It can be seen from the above equation that the right side of the equation contains the translation and the rotation components, and they are independent of each other. Using brute force sampling, each particle state needs to calculate the rotation and the translation components. Additionally, if we first uniformly sample the angle as the base and then perform uniform sampling on the translation, we only need to perform the corresponding translation (involving the addition operation) on the basis of the rotation, which will greatly simplify the calculation. The particle sampling process based on the correlation method is shown in Figure 4.  
Step 1 means using the robot's internal sensors such as the wheel odometer and the IMU to obtain the proposal pose of the robot. According to the laser measurement, the maximum distance measured by the laser is obtained and the angle step is calculated as \(\Delta\theta=1-\arccos(\frac{1}{z_{\max}})\). The number of rotation samples can be calculated according to the angle search window set by the algorithm. As shown in step 2, there are 11 rotation directions. In each rotation direction, the coordinates of the laser measurement point in the world coordinate system are obtained and recorded. In step 3, according to the translation search window size and the sampling interval set by the algorithm, the number of sampled particles for translation can be calculated. In Figure 4, three particle samples are set in the \(x\) and the \(y\) directions for each orientation. According to the coordinates of the laser measurement data saved in each orientation in step 2 in the world coordinate system plus the corresponding offset in the \(x\) and the \(y\) directions, the coordinates of the  
Figure 4: Correlative particle sampling.  
laser measurement points of all sampled particles in the world coordinate system can be obtained. Then, we can use Formula (7) to calculate the corresponding grid coordinates.  
\[\left\{\begin{array}{l}x_{\frac{x_{t}^{k}}{t}}^{index}=(x_{x_{t}^{k}}-x_{origin}) /scale\\ y_{z_{t}^{k}}^{index}=(y_{z_{t}^{k}}-y_{origin})/scale\end{array}\right. \tag{7}\]  
where \((x_{origin},y_{origin})\) is the coordinate of the grid map origin in the world coordinate system and \(scale\) is the resolution of the grid map. According to the grid coordinates of the laser measurement point, the score information can be directly obtained from the grid map through the index and the sample particle with the highest score can be found. The optimal pose estimation can be obtained by adding the offset corresponding to the highest scoring particle to the initial proposal pose.