# Extended Particle-Aided Unscented Kalman Filter Based on Self-Driving Car Localization
## Extended Particle-Aided Unscented Kalman Filter
### Particle Weight Estimation Based on Three-Dimensional Features  
Weight estimation, also known as sequential importance sampling (SIS), is a critical step in that it directly affects the importance of each particle from the prediction step. The particle filter is an application of an SIS approach. The main idea of weight estimation is to represent the required posterior density function by a set of random samples, which are taken from the prediction step. These samples use associated weights to compute estimates based on these samples and weights [28]. It is necessary to calculate the error of the predicted mean value and the measured value of each particle, in order to calculate the weight. On the basis of the error of predicted and measured values, the weight of each particle can be calculated. Since the predicted value is calculated in the prediction step, the only parameter that affects weight is the measurement value. In this paper, the extended PAUKF considered the three-dimensional features from the perception module, such as LiDAR- andRADAR-based object recognition. Since the range sensor calculated the distance \(\mathrm{d_{i}}\) between features based on time of flight, the measured distance is the point-to-point distance, as Figure 2 shows. The method differs from the two-dimensional case. Because of the vertical offset of the features, the weight calculation should also be extended to a higher dimension. The only known measurement values are the point-to-point relative distance \(\mathrm{d_{i}}\) between vehicle and feature F as well as the angle of the relative bearing \(\alpha_{i}\) and relative elevation \(\beta_{i}\), as Figure 2 shows. The geometry perception data should be considered as well as the noise affection in the vertical direction. The measurement value of the vehicle is shown as Equation (5), since sensors in the real world cannot precisely recognize out-ranged targets. Therefore, a valid recognition limit of 50 m was set, meaning that the vehicle localizes itself based on features within 50 m.  
\[ \begin{split}\text{If }\mathrm{d_{i}}<50,\;\text{Z}_{k+1}\;=\; \left[\begin{array}{c}\mathrm{d_{[i]}}\\ \Delta\alpha_{[i]}\end{array}\right]_{k\pm 1}=\\ \left[\begin{array}{c}\sqrt{\left(\mathrm{x}_{\mathrm{f},\mathrm{i}, \mathrm{k}+1}-\mathrm{x}_{\mathrm{v}}\right)^{2}+\left(\mathrm{y}_{\mathrm{f}, \mathrm{i},\mathrm{k}+1}-\mathrm{y}_{\mathrm{v}}\right)^{2}+\left(\mathrm{z}_{ \mathrm{f},\mathrm{i},\mathrm{k}+1}-\mathrm{z}_{\mathrm{v}}\right)^{2}}\\ \arctan\left(\frac{\mathrm{x}_{\mathrm{f},\mathrm{i},\mathrm{k}+1}-\mathrm{x}_ {\mathrm{v}}}{\mathrm{y}_{\mathrm{f},\mathrm{i},\mathrm{k}+1}-\mathrm{y}_{ \mathrm{v}}}\right)\end{array}\right]_{k\pm 1}+\left[\begin{array}{c}\epsilon_{\mathrm{d}}\\ -\mathrm{\theta}_{\mathrm{v}}+\epsilon_{\Delta\alpha}\end{array}\right]_{k+1}. \end{split}  \tag{5}\]  
The perception values are transformed based on the generated particle value \(\overline{\mathrm{X}}_{k+1}\), which is represented as \(\overline{\mathrm{x}}_{\mathrm{f},\mathrm{k}+1}\), \(\overline{\mathrm{y}}_{\mathrm{f},\mathrm{k}+1}\) and \(\overline{\mathrm{z}}_{\mathrm{f},\mathrm{k}+1}\). As the feature extends to three dimensions, an extended method should be used to evaluate each weight. After unifying the measurement value and the predicted value in map coordinates, the multivariable normal distribution was used for weighting particles, as Equation (6) shows. The highest weighted particle was used for estimating the position. The total particle number N is 100. \[