This section describes the implementation of the PAUKF, including particle implementation and PAUKF implementation. Both the PF and UKF are Bayesian-based filters, and the environment is assumed to be Markov, which means that the PAUKF also has a Markov assumption.
2.1. Particle Filter Algorithm
The particle filter is a Monte Carlo-based method that can handle both Gaussian noise and non-Gaussian noise [
42]. Because the vertical movement of the vehicle is small, we only consider the vehicle in a two-dimensional Cartesian space with the vehicle heading θ. A bicycle model is used in this study to represent the motion of the vehicle because a complex vehicle model aggravates the computational burden, and many parameters cause additional noise [
43]. The state of the vehicle is represented by <x, y, and θ>, as shown in
Figure 1.
The inputs that we use are the range sensor and the ground truth of the infrastructures in the HD map. The final position of the vehicle should be the best posterior belief based on past data and the current state. The particle filter is a nonparametric implementation of the Bayes filter, which uses a finite number of samples to approximate the posterior. Thus, the final belief bel(x) should be generated for each particle by using each important factor (weight), as shown in Equation (1). The
means the state vector of each particle and
is the weight of each particle. The size of each particle X was 3 × 1. W is a non-negative factor termed as the importance factor. In this study, we used 100 particles for simulation, which means that N is 100. The larger the importance factor, the more it affects the final estimation result.
The particle filter for localization can be divided into four parts, which are introduced in the following subsections.
2.1.1. Initialization
To localize the vehicle in global coordinates, it is essential to provide an initial location to the vehicle. Otherwise, the vehicle will search for its position over the entire world. Therefore, we use the GPS sensor for initialization. Even though the GPS signal is poor due to multipath and blocking issues, it still provides a limited area for the vehicle to localize. Because the particle filter is recursive, after initialization, the noisy GPS signal data are filtered recursively. When a particle filter receives GPS data, it generates N random particles for initialization.
2.1.2. Prediction
To obtain the prior belief, each particle at timestamp k − 1 should predict the current state based on the system prediction model. The prediction model was constructed based on the vehicle model. Complex models, such as a dynamic model with tires, can also be included. However, complex vehicle models reduce computation efficiency. Such models also require detailed vehicle parameters, which are difficult to set. Incorrect parameters can cause noisy estimations. Considering the computational burden and precision, a kinematic model was used in this study [
43]. We ignore the slip angle because vehicle travel in cities is typically not fast.
As Equation (2) shows, the prediction contains several trigonometric functions, which correspond to a highly nonlinear prediction model. The theta angle of each particle is critical because it changes according to the local vehicle coordinates. The kinematic model incorporates several assumptions such as the value of being equal to zero.
2.1.3. Weight Calculation
Weight is also an important factor that can heavily influence particle motion. Measurements from the range sensor were used to calculate the weight. We assume that the vehicle can receive all the data from the vehicle-to-everything (V2X), HD map, and range sensor. Thus, the vehicle can receive the distance data and orientation between the vehicle and every exterior infrastructure. Here,
is the measured distance between the vehicle and infrastructure i,
is the distance measurement noise, and
is the relative orientation angle of the vehicle and infrastructure i. As Equations (3) and (4) show, the measurement model is nonlinear in nature.
To evaluate the weight, a multivariable normal distribution function was used to assess the importance of each particle. Thus, a multivariable normal distribution function returns the weight of a particle based on the newest sensor measurement values and the predicted values from the model as
2.1.4. Resampling
After calculating the weight and prediction values, the particle filter should select the particle along with its corresponding weight, which is the resampling step.
The entire PF part of the algorithm’s pseudo code is shown in
Table 1.
2.2. Particle-Aided Unscented Kalman Filter Algorithm
The particle filter algorithm is introduced in
Section 2.1. The particle estimates the position of the vehicle by using the range sensor. The final results for each particle contain information about the surrounding infrastructures and the position of the ego vehicle. Therefore, it can be concluded that this sensor provides more accurate results. When the UKF estimates the state, the results from the particle filter will be the measurement value of the vehicle. Subsequently, the PAUKF can extract a more precise result based on the particle filter estimation results. A flowchart of the PAUKF is shown in
Figure 2.
The UKF is a Bayesian filter that has better performance than the EKF when estimating the state of a discrete-time nonlinear dynamic system. Because it is based on a Kalman filter, the framework of a UKF is almost the same as that of a KF. The difference is that a UKF performs stochastic linearization by using the weighted statistical linear regression process, known as an unscented transform. Instead of using a Taylor expansion, a UKF deterministically extracts the mean and covariance using the sigma points.
The sigma points are predefined by an empirical parameter
that is calculated using Equation (6). The sigma point is a symmetrical region around the mean value.
is the covariance matrix of the state, which updates at every iteration. The state vector of the vehicle is
, which is a 5 × 1 vector, as shown in Equation (7). The state vector value is the mean of the sigma matrix.
is the quantity of the state vector with a size of 5. Then, the sigma points are generated using Equation (8).
After it generates the sigma points, a UKF needs a prediction model to determine the prior probability of the state. To obtain a more precise position regarding position, a UKF considers more states of the vehicle and the effect of nonlinear noise on the states.
The constant turn rate and velocity magnitude (CTRV) vehicle motion model was used in this study [
44]. Based on the CTRV model, the discrete state transition model is derived by integrating the differential equation of the state, which considers the nonlinear process noise vector. Considering the underlying structure of the vehicle for a real-world test, the acceleration and yaw acceleration are considered to be noise. In particular, the acceleration and yaw acceleration noise effects are nonlinear. Therefore, the process noise cannot be handled by addition alone. In order to handle nonlinear noise, it is considered to be a state, as shown in Equation (9). This means that the size of
becomes
, which is a 7 × 1 vector.
The process noises
and
are set as normal Gaussian distributions with variances of
and
, respectively, as shown in Equations (10) and (11).
The covariance matrix
is also augmented into
, which has a size of 7 × 7, as shown in Equation (12).
The process model is derived based on the CTRV assumption and noise, as shown in Equation (13). The model that we derive has highly nonlinear properties, as indicated in Equation (14).
In the augmented prediction step,
, should be the quantity of the augmented state
with a size of 7, and
also needs to be calculated as in Equation (15). Subsequently, the sigma points are predicted using Equation (16).
The weight of each sigma point is calculated based on Equations (17) and (18). The predicted mean and covariance were calculated using Equations (19) and (20). The predicted value is the prior of the Bayesian distribution model. These predicted values should be updated when the measurement data are incoming.
After predicting the new mean and covariance matrix based on the augmented sigma point, the algorithm no longer needs to consider noise as the acceleration noise information is already included in the state. Therefore, the augmented state changes back to a normal state with a size of 5. Until now, the prior probability was calculated based on sigma points. When using a Bayesian filter, measurement prediction can be implemented. Instead of using the original range sensor with noise from the vehicle,
is used in this step. This means that
becomes a virtual sensor, which is more precise than the original sensor. Since
already includes sensor information based on the particle filter, it optimally provides a more precise belief of the state. The measurement vector of the sensor is shown in Equation (21).
The measurement model is shown in Equations (22) and (23). The particle filter measurement provides x, y, and yaw data. This means that the number of rows in matrix A is 3. Because the augmented state information is included in the state, the state of the UKF recovers to 5. This means that the number of columns in matrix A is 5.
The predicted measurement mean is calculated based on the weight of each measurement’s sigma points, as shown in Equation (24).
The predicted measurement covariance is calculated using Equation (25). R is the measurement noise covariance, as shown in Equation (26). The covariance is tuned according to the particle filter estimation results.
At this time, a measurement value is needed to calculate the posterior probability. The update step is similar to that of the Kalman filter. The only difference is that the UKF needs to calculate the cross-correlation value, according to Equation (27), between the sigma points in the state space and the measurement space.
Based on the cross-correlation matrix and the measurement covariance, the Kalman gain is then calculated as
The state is updated using the measurement value
, which is obtained from the particle filter estimation as
The covariance matrix is then updated based on the updated Kalman gain and the measurement covariance matrix as
The terms
and
are the final estimation results of the PAUKF, which combines the bicycle model, CTRV motion model, Monte Carlo-based estimation, and unscented Kalman filter-based estimation. The complete pseudo code of the PAUKF algorithm is shown in
Table 2.