# Research on Map-SLAM Fusion Localization Algorithm for Unmanned Vehicle
## 3 Fusion of Lane Location Information and SLAM Positioning Algorithm
### Location Probability Update Based on Lane Line Observation  
As shown in Figure 5. This link needs to select the \(K\) points with the highest probability among the predicted candidate points. Since the lane of the map has been discretized by these points, first find the only corresponding lane line point \(map_{n}\) in the tangent direction of the vehicle attitude. This point is in the lane line stored in the original directed graph, representing the projection of the vehicle's position on the lane line map point at this time. Observation is carried out at each candidate point, and the observed value is the distance \(d_{lane}\), the angle of the lane line \(\theta_{lane}\) and the position of the vehicle body, which is obtained by the local positioning method of the lane line. The points in the map save the position information \((x,y)\) and tangential angle information \(\theta_{km}\) of each point, which can be combined with \(d_{lane}\) and angle \(\theta_{lane}\). According to the current position of the vehicle, the distance to the map point in the direction of the tangential lane line can be obtained \(d_{km}=\sqrt{\left(x_{k}-map_{nx}\right)^{2}+\left(y_{k}-map_{ny}\right)^{2}}\), \(map_{n}\) represents the currently calculated map candidate point. \(\theta_{lane}\) represents the angle between the vehicle and the lane line. Set the current yaw angle of the vehicle as that which is output by the SLAM system and converted into world coordinates in the initialization process. At this time, the calculated lane angle is \(\theta_{mapn}=\theta_{lane}+\theta_{k}\). It is worth noting that the lane line detection section above has fitted the lane line to the lane line closest to the current vehicle, so the probability of the vehicle at the current candidate point can be described by the probabilistic equation of the above parameters. Figure 4: Probabilistic update diagram. Figure 5: Recalculation of positioning information combined with map information. \[ \begin{array}{c}P_{X_{k+1}}^{k+1}=P_{X_{k1}}^{k_{k+1}}*\lambda*\omega\exp\left(- \frac{1}{2}Y^{T}*D^{*}Y\right)\\ Y=\left\{x-x_{k+1},y-y_{k+1},d_{kn}-d_{lame},\theta_{kn}-\theta_{mapm}\right\} \\ \sum\limits_{n=1}^{K}P_{X_{k+1}}^{k+1}=1\\ \lambda=\left(\frac{1}{2\pi}\right)^{2}|D|^{-1/2}\end{array}  \tag{12}\]  
\(P_{X_{k+1}}^{k+1}\) represents the probability of the latest position candidate point obtained by the vehicle after the above prediction, which is a correct update of \(P_{X_{k}}^{k+1}\). The observations \(Y\) used to describe the vehicle at this candidate point include the latitude and longitude position \((x,y)\), the map distance point from the vehicle to the lane line, and the current. The error of the local positioning result \((d_{kn}-d_{lane})\), as well as the error of the angle \((\theta_{kn}-\theta_{mapm})\) stored by the candidate point and the current observation angle, are a total of four-dimensional variables. The matrix \(D\) represents the covariance matrix. This paper assumes that each variable is distributed independently. \(D\) is a diagonal matrix with a dimension of \(4\times 4\), and the four diagonal corners \((d_{x},d_{y},d_{\theta},d_{l})\) are, respectively, from the upper left to the lower right corner. \((d_{x},d_{y})\) is the position variance, \(d_{\theta}\) is the angle variance with In radians and \(d_{l}\) represents the error of the current local positioning. Due to abnormal conditions such as detection failure, a robust kernel function \(h(x)\) is added to prevent the unstable influence caused by the wrong local positioning, and it is eliminated when the error is too large. Finally, all the candidate point probabilities are updated. \[D=\left| \begin{array}{c}d_{x},0,0,0\\ 0,d_{y},0,0\\ 0,0,d_{\theta},0\\ 0,0,0,d_{l}\end{array} \right| \tag{13}\]  
In this paper, only one lane line is used as the observation. In the experiment, the accuracy improvement produced by the use of multiple lane lines is very small, and at the same time, it generates a lot of problems, so that the data association difficult to determine.