Vehicle Trajectory Prediction Method Based on “Current” Statistical Model and Cubature Kalman Filter

: Vehicle motion trajectory prediction is the basis of vehicle collision early warning or vehicle conﬂict resolution. In order to improve the accuracy of trajectory prediction, a vehicle trajectory prediction method based on “current” statistical (CS) model and cubature Kalman ﬁlter (CKF) is proposed. This method considers the acceleration variation rules in the actual motion process of the vehicle in the state equation, so that the estimated value of the acceleration can be consistent with the real range. This condition overcomes the limitation of the general trajectory prediction model, which ignores the acceleration change, so it improved prediction accuracy. In addition, this method also avoids the large amount of computational resources required, being that some new methods describe the real acceleration ﬂuctuations. The vehicle trajectory at the intersection that crossed by Yingbin Avenue and Qiche Avenue in Nanchang is selected to verify the tracking performance of Constant Acceleration-Unscented Kalman Filter (CA-UKF), Current Statistical-Unscented Kalman Filter (CS-UKF), and CS-CKF models. The results show that the CS-CKF model has superior prediction effectiveness than the CA-UKF and CS-UKF models, and it improves the accuracy of vehicle motion trajectory prediction.


Introduction
With the continuous development of autonomous driving and other emerging technologies, autonomous vehicles must navigate an environment full of human drivers [1]. The increasing complexity of China's road traffic environment has become an inevitable trend, resulting in frequent traffic accidents, and the problem of traffic safety has also entered the public's vision [1]. Vehicle trajectory prediction is an essential prerequisite for vehicle collision avoidance and is an effective means to ensure vehicle driving safety [2]. At the current state of technology, the permeability of autonomous vehicles on the road is still relatively low, and the ideal state of vehicle-vehicle connectivity is still difficult to achieve. Therefore, how to use trajectory prediction technology to reduce the incidence of traffic accidents and to ensure the safe driving of autonomous vehicles in the current environment is an important topic for further development of autonomous driving technology.
Vehicle trajectory prediction is a problem of moving target tracking, which is inseparable from vehicle motion model and filtering algorithms. Regarding the motion model, in recent years, many scholars have conducted relevant studies on target tracking. W. Blair [3] constructed a novel joint estimation algorithm, based on the Constant Velocity (CV), of the vehicle. This model ignores the acceleration of the vehicle and is constrained in the estimation process. Different from the CV model, the Constant Acceleration (CA) model [4] adds the acceleration of a moving target as the system state parameter. However, the acceleration is constant, and the possible changes in acceleration are disregarded in the estimation process. In 1970, R. Singer [5] proposed a time-dependent model. This model assumes that the acceleration of the target is time-dependent and stable, which overcomes the acceleration of the motion model as a constant value in the previous research has certain hidden dangers. From the perspective of the actual running state of the vehicle, neither the uniform motion, nor the uniform acceleration motion, can accurately describe the actual driving state of the vehicle, which eventually leads to a large deviation in the calculated trajectory.
Based on the inadequacies of previous studies, we proposed a CS-CKF vehicle trajectory prediction model on the basis of the research results of the literature [6], combined with the CKF. This model integrates the accuracy of CS model and the stability and real-time of CKF algorithm and addresses the problem that the vehicle acceleration change or the acceleration change range is extremely large in the conventional motion model, leading to the time-consuming calculation process of the filtering algorithm. The vehicle motion state is described by CA and CS, and the three prediction models of CA-UKF, CA-CKF, and CS-CKF are simulated by the MATLAB program. The simulation results show that the performance of the CS-CKF model proposed in this paper is better than the other two trajectory prediction models.
In this paper, the CS model, considering the acceleration change, is used as the basic motion model, which solves the large error problem caused by ignoring the acceleration change in previous studies. In the choice of filtering algorithm, in order to better deal with the nonlinear problem caused by the Rayleigh distribution of acceleration, CKF is selected as the solution algorithm. It reduces the cumbersomeness of the calculation process, has less dependence on the calculation power, and has a wider range of operations in practical applications.

Vehicle Trajectory Prediction Process
When the KF filter algorithm is used to predict the vehicle trajectory, it is divided into two parts: one is the vehicle position probability distribution predicted by the state equation, and the other is the vehicle position probability distribution obtained by the measurement equation. The standard weighted Gaussian integral of the function is calculated using the third-order volume integral principle, and the two parts of the probability distribution are fused to obtain the predicted probability distribution of the vehicle position at the next moment. The schematic diagram of the filtering algorithm is shown in Figure 1 [16].
According to the latest research results of fault tree analysis theory [15] to analyze the smooth operation of incoming vehicles, it can be known that a small change in acceleration may cause a major traffic accident in some specific occasions. Therefore, the research method of setting the acceleration of the motion model as a constant value in the previous research has certain hidden dangers. From the perspective of the actual running state of the vehicle, neither the uniform motion, nor the uniform acceleration motion, can accurately describe the actual driving state of the vehicle, which eventually leads to a large deviation in the calculated trajectory.
Based on the inadequacies of previous studies, we proposed a CS-CKF vehicle trajectory prediction model on the basis of the research results of the literature [6], combined with the CKF. This model integrates the accuracy of CS model and the stability and realtime of CKF algorithm and addresses the problem that the vehicle acceleration change or the acceleration change range is extremely large in the conventional motion model, leading to the time-consuming calculation process of the filtering algorithm. The vehicle motion state is described by CA and CS, and the three prediction models of CA-UKF, CA-CKF, and CS-CKF are simulated by the MATLAB program. The simulation results show that the performance of the CS-CKF model proposed in this paper is better than the other two trajectory prediction models.
In this paper, the CS model, considering the acceleration change, is used as the basic motion model, which solves the large error problem caused by ignoring the acceleration change in previous studies. In the choice of filtering algorithm, in order to better deal with the nonlinear problem caused by the Rayleigh distribution of acceleration, CKF is selected as the solution algorithm. It reduces the cumbersomeness of the calculation process, has less dependence on the calculation power, and has a wider range of operations in practical applications.

Vehicle Trajectory Prediction Process
When the KF filter algorithm is used to predict the vehicle trajectory, it is divided into two parts: one is the vehicle position probability distribution predicted by the state equation, and the other is the vehicle position probability distribution obtained by the measurement equation. The standard weighted Gaussian integral of the function is calculated using the third-order volume integral principle, and the two parts of the probability distribution are fused to obtain the predicted probability distribution of the vehicle position at the next moment. The schematic diagram of the filtering algorithm is shown in Figure 1 [16].  In Figure 1, X k+1 is the state variable of the vehicle at the k + 1 moment, Z k is the measurement variable, P k is the probability density, and Q and R are process noise and measurement noise, respectively.

Vehicle Motion Model
The vehicle motion model is the digital embodiment of vehicle motion state. The more consistent the model is with the actual situation, the better the vehicle tracking effect is. The vehicle motion state is shown in Figure 2.

Vehicle Motion Model
The vehicle motion model is the digital embodiment of vehicle motion state. more consistent the model is with the actual situation, the better the vehicle tracking e is. The vehicle motion state is shown in Figure 2.  Figure 2 shows the difference in the two vehicles' motion states. In the two-dim sional Cartesian coordinate system, the sampling interval is T, assuming that the targ a particle and ignoring the influence of the overall size of the target on the motion s Point A in the figure represents the position of the moving target at the time . At time, the state variable of the vehicle can be expressed as , , , , , , each parameter represents the position, velocity, and acceleration of the target on th axis and y-axis. At the 1 moment, the actual position of the vehicle is point C, and state variable can be expressed as . Points B and D represent the vehicle positio timated by the CA model and CS model at the 1 moment, respectively. In accordance with the definition, the CA model refers to taking the target mo state as a uniform acceleration motion with constant acceleration. At this time, the a eration of the target is constant, and according to the classical Kalman algorithm, th hicle equation of state can be expressed as Equation (1).
where is the state variable of the vehicle at the 1 moment, Φ is the sy state transition matrix, and is the state noise. In Equation (1), the acceleration is always conceded as constant. However, the a eration of the moving target changes with the maneuvering time in practice, and th celeration change value of the general target at the next time is within a certain rang all possible values of the acceleration are considered in the estimation, then the realperformance of the calculation will be reduced. According to the research results of fessor Zhou [6], we regard the overall acceleration distribution in the vehicle motion as a modified Rayleigh distribution. The average acceleration is the estimated value o acceleration at the current time and is not constant to zero. In accordance with the v tion characteristics of acceleration, it can be seen that the CS model can improve th sponse speed of filtering algorithm to moving target tracking to a certain extent an more in line with the actual moving state of target in maneuvering.  Figure 2 shows the difference in the two vehicles' motion states. In the two-dimensional Cartesian coordinate system, the sampling interval is T, assuming that the target is a particle and ignoring the influence of the overall size of the target on the motion state. Point A in the figure represents the position of the moving target at the time k. At this time, the state variable of the vehicle can be expressed as In accordance with the definition, the CA model refers to taking the target motion state as a uniform acceleration motion with constant acceleration. At this time, the acceleration of the target is constant, and according to the classical Kalman algorithm, the ve-hicle equation of state can be expressed as Equation (1).
where X k+1 is the state variable of the vehicle at the k + 1 moment, Φ is the system state transition matrix, and W k is the state noise. In Equation (1), the acceleration is always conceded as constant. However, the acceleration of the moving target changes with the maneuvering time in practice, and the acceleration change value of the general target at the next time is within a certain range. If all possible values of the acceleration are considered in the estimation, then the real-time performance of the calculation will be reduced. According to the research results of Professor Zhou [6], we regard the overall acceleration distribution in the vehicle motion state as a modified Rayleigh distribution. The average acceleration is the estimated value of the acceleration at the current time and is not constant to zero. In accordance with the variation characteristics of acceleration, it can be seen that the CS model can improve the response speed of filtering algorithm to moving target tracking to a certain extent and is more in line with the actual moving state of target in maneuvering.
In the CS model, the effect of acceleration change on the object state needs to be considered, and the discrete state equation of the moving target can be expressed as Equation (2). Unlike the CA model, the acceleration of the moving target in the CS model is irregular and changes with time. At this time, the system state transition matrix Φ is expressed as Equation (3).
where; U k is the input matrix, and the value is expressed as Equations (4) and (5).
where; W k is the state noise, with mean value of zero and variance of Q k = 2ασ 2 α q, α is the target maneuver frequency, q is a constant matrix, and its value can be found in the literature [17]. The variance of maneuvering acceleration is σ 2 α , and its value is determined by the mean acceleration of x-axis and y-axis, which can be expressed as Equation (6).
where a max , a −max , and − a k are the limit value and average value of acceleration. The average value of acceleration is the one-step predicted value of target acceleration, which can be expressed as Equation (7).
Equations (3)-(7) draw on Professor Zhou's 1984 study on the Kalman filter algorithm for adaptive mean and variance of mechanical acceleration. The change in vehicle acceleration is considered in the CS model and fluctuates within a certain range in accordance with the statistical value. Thus, it is more reasonable to describe the vehicle motion state, which is conducive to the tracking and prediction of vehicle trajectory.

Measurement Model
As a traffic infrastructure, radar detectors have been widely used in road traffic systems. The radar can detect the position, distance, speed, and other information of the vehicle. It has the advantages of long detection distance, high precision, and can quickly and accurately obtain the spatial distance or other information in the traffic scene [18]. Therefore, this model is measured in radar environment.
In the two-dimensional coordinate system, the positioning of the object needs to use the coordinate origin and the included angle. According to the model of the classical discrete linear dynamic system of the Kalman filter algorithm, the measurement model of the object at this time can be expressed as Equation (8). where; Z k is the measurement variable, h[•] is the measurement equation, r(k) is the radial distance between the target and the observation point, ϕ(k) is the included angle between the target and the observation point, and v(k) is the measured noise. According to the data characteristics of the measurement parameters and the spatial coordinate relationship between the measurement targets, the expressions of r(k) and ϕ(k) are derived, as shown in Equation (9).
where; x(0), y(0) are the position coordinates of the origin point.

CKF Algorithm
CKF is a Bayesian algorithm that mainly calculates the posterior probability density function through the initial state and measured values of the system [19]. According to the research of Yazdi et al., the Bayesian algorithm and its extension algorithm have good adaptability when dealing with fault monitoring and multi-factor fault analysis [20]. This research plays a key role in analyzing the internal mechanism of acceleration affected by various noise factors. In the calculation, it uses the third-order cubature criterion and Gaussian quadrature criterion to estimate the nonlinear system, similar to the UKF. However, different from UKF, CKF uses m cubature points with equal weight (m = 2n, n is the dimension of state vector), which is one less than the number of cubature points in UKF [21], which is easier to calculate. Thus, it has stronger real-time performance in trajectory prediction.
As mentioned previously, we mainly use the CS model to describe the vehicle motion state. In the CS model, the dimension of system state vector is 6, which belongs to a high-dimensional nonlinear system. In accordance with the characteristics of UKF and CKF, CKF has higher accuracy and better real-time performance in high-dimensional nonlinear systems. Therefore, we selected the CKF algorithm to predict the vehicle trajectory.
The estimation process of CKF includes two parts: time update and measurement update. The main process is shown in Figure 3.  Define the state vector as , covariance as , and the process and measurement noises as and .

Initialization:
Define the state vector asx k , covariance as P k , and the process and measurement noises as Q and R.
Calculate the predicted value of state value and covariance: where; S k is the square root of P k , n is the dimension of the state vector, is the set of cubature point, x i,k+1|k is the cubature point at which the output is propagated through the state equation,x k+1|k is the one-step predicted value of the state vector, and P k+1|k is one-step predicted value of error covariance.

7.
Calculate the predicted value of the measurement:

8.
Calculate the measurement error covariance and cross covariance: 9. Calculate the Kalman gain and update the state quantity and the corresponding error covariance: Electronics 2023, 12, 2464 8 of 13 where; S k+1|k is the square root of P k+1|k , x i k+1|k is the calculated cubature point, Z i k+1|k is the cubature point at which the output is propagated through the measurement equation, z k+1|k is the predicted value for the next step of measurement, P z k+1 is the information covariance matrix, P xz k+1 is the one-step predicted value of the cross covariance matrix, K k+1 is the Kalman gain matrix,x k+1|k is the state vector at the current time, and P k+1|k is the error covariance matrix at the current time.

Simulation Results and Analysis
The method of CA-UKF, CS-UKF, and CS-CKF (this paper proposed) models are applied to track and predict the vehicle maneuvering state. As the intersection is a high incidence place of traffic accidents, tracking the vehicle trajectory in the intersection has more practical importance [22]. We selected the intersection of Yingbin Avenue crossed Qiche Avenue in Nanchang as the research object. The proposed method is applied to predict the vehicle trajectory by the Matlab program. In order to compare the performance of tracking prediction, the CA-UKF, and CS-UKF models are also applied. The intersection scene is shown in Figure 4 9. Calculate the Kalman gain and update the state quantity and the corresponding error covariance: where; | is the square root of | , | is the calculated cubature point, | is the cubature point at which the output is propagated through the measurement equation, ̂ | is the predicted value for the next step of measurement, is the information covariance matrix, is the one-step predicted value of the cross covariance matrix, is the Kalman gain matrix, | is the state vector at the current time, and | is the error covariance matrix at the current time.

Simulation Results and Analysis
The method of CA-UKF, CS-UKF, and CS-CKF (this paper proposed) models are applied to track and predict the vehicle maneuvering state. As the intersection is a high incidence place of traffic accidents, tracking the vehicle trajectory in the intersection has more practical importance [22]. We selected the intersection of Yingbin Avenue crossed Qiche Avenue in Nanchang as the research object. The proposed method is applied to predict the vehicle trajectory by the Matlab program. In order to compare the performance of tracking prediction, the CA-UKF, and CS-UKF models are also applied. The intersection scene is shown in Figure 4.  In Figure 4, the selected intersections are regular intersections, and the intersecting roads are urban arterial roads. When collecting data, the intersection range captured by the UAV is large enough, and the trajectory length of the vehicle passing through the intersection can meet the requirements of the number of sampling points.
The unmanned aerial vehicle hovers at a height of 100 m to record the traffic video of the intersection. Assume that the unmanned aerial vehicle position is fixed and do not consider its slight jitter. The video duration is 6 min, and Pix4Dmapper is used to output 361 image files at 30 frame intervals. The left turning vehicle track of the north entrance road from time 3:28 to time 3:58 in the video is selected as an example for specific analysis. The sampling time is 30 s, and the sampling interval is 1 s. The vehicle positions of some sampling track points are shown in Figure 5. Figure 5 shows the vehicle positions of the left turning vehicle at the starting time, 10 s, 20 s, and 30 s. The pedestrian crossing line on the road surface is selected as the fixed feature point to calibrate the video coordinate points of the research vehicle, and the pixel coordinates of the track points are transformed into world coordinates [23]. In the two-dimensional coordinate system, the observation position is the coordinate origin (0, 0), which the virtual radar is set at. The distance and deflection angle between radar and target vehicle at each sample point can be calculated through coordinate spatial relationship from radar and target. It is assumed that the influence of radar height, vehicle height, and possible shelter is not considered, besides the radar angle of pitch. In order to simulate the measurement system error of the radar, the noise with normal distribution is added to each group of observation data. The distribution parameters of measured distance and angle are N 0, σ 2 r , N 0, σ 2 θ . The initial state of the target is X 0 , and the radial distance and included angle from the observation parameter are r k and ϕ k and its time stamp, respectively. The measurement noise covariance is R k = diag σ 2 r , σ 2 θ , σ r = 5 m, and σ θ = 0.04 rad. The sampling period T = 1 s, the total running time is 30 s, and the Monte Carlo simulation times are 100 s. In the CS model, the target maneuvering frequency α = 0.05, and the limit value of maneuvering acceleration a = 1 m/s 2 . In the UKF filtering algorithm, α = 0.01, β = 2, and κ = 0.
Electronics 2023, 12, x FOR PEER REVIEW 9 of 13 In Figure 4, the selected intersections are regular intersections, and the intersecting roads are urban arterial roads. When collecting data, the intersection range captured by the UAV is large enough, and the trajectory length of the vehicle passing through the intersection can meet the requirements of the number of sampling points.
The unmanned aerial vehicle hovers at a height of 100 m to record the traffic video of the intersection. Assume that the unmanned aerial vehicle position is fixed and do not consider its slight jitter. The video duration is 6 min, and Pix4Dmapper is used to output 361 image files at 30 frame intervals. The left turning vehicle track of the north entrance road from time 3:28 to time 3:58 in the video is selected as an example for specific analysis. The sampling time is 30 s, and the sampling interval is 1 s. The vehicle positions of some sampling track points are shown in Figure 5.   [23]. In the twodimensional coordinate system, the observation position is the coordinate origin (0, 0), which the virtual radar is set at. The distance and deflection angle between radar and target vehicle at each sample point can be calculated through coordinate spatial relationship from radar and target. It is assumed that the influence of radar height, vehicle height, and possible shelter is not considered, besides the radar angle of pitch. In order to simulate the measurement system error of the radar, the noise with normal distribution is added to each group of observation data. The distribution parameters of measured distance and angle are 0, , 0, . The initial state of the target is , and the radial distance and included angle from the observation parameter are and and its time stamp, respectively. The measurement noise covariance is , , 5 m , and 0.04 rad. The sampling period 1 s, the total running time is 30 s, and the Monte Carlo simulation times are 100 s. In the CS model, the target maneuvering frequency 0.05, and the limit value of maneuvering acceleration 1 m/s . In the UKF filtering algorithm, α 0.01, β 2, and κ 0. The simulation is conducted in accordance with the above algorithm and parameter conditions, a program is developed in MATLAB, and the tracking results are shown in Figure 6. The simulation is conducted in accordance with the above algorithm and parameter conditions, a program is developed in MATLAB, and the tracking results are shown in Figure 6. As shown in Figure 6, the predicted trajectories of CA-UKF, CS-UKF, and CS-CKF models fit the real trajectories. Figure 6a shows the tracking comparison between CA-UKF and CS-UKF and the real trajectory. Since the CS model takes into account the instantaneous change in acceleration when the vehicle starts, the trajectory points predicted in the As shown in Figure 6, the predicted trajectories of CA-UKF, CS-UKF, and CS-CKF models fit the real trajectories. Figure 6a shows the tracking comparison between CA-UKF and CS-UKF and the real trajectory. Since the CS model takes into account the instantaneous change in acceleration when the vehicle starts, the trajectory points predicted in the initial stage are closer to the real trajectory than those predicted by the CA model. Further analysis, based on the CS model, the tracking effects of CKF and UKF are compared in Figure 6b. The simulation results show that the CKF algorithm is better than the UKF algorithm in predicting the trajectory point fitting trajectory based on the CS model. Combined with the comparison results of the two prediction trajectories, the prediction effect of the CS-CKF model is better than that of the CA and UKF models.
The root mean square errors (RMSE) of predicted position, velocity, and acceleration of moving targets are calculated to more intuitively analyse the accuracy of trajectory prediction results. The calculation formulas are expressed in Equation (24).
where; P xi , P yi is the prediction value of x and y directions at ith point, P xi , P yi is the actual value at ith point, and N is the number of track points. By applying the point predic- The RMSE results are shown in Figure 7. In accordance with the RMSE of vehicle position, speed, and acceleration prediction in Figure 7, the predicted value of vehicle position of the three models is greatly different from the real value at the initial time, and gradually tends to be stable. Figure 7a shows the RMSE of the predicted position. When using the UKF algorithm, the errors of CA-UKF In accordance with the RMSE of vehicle position, speed, and acceleration prediction in Figure 7, the predicted value of vehicle position of the three models is greatly different from the real value at the initial time, and gradually tends to be stable. Figure 7a shows the RMSE of the predicted position. When using the UKF algorithm, the errors of CA-UKF and CS-UKF models are similar, but the data distribution range shows that the overall stability of CA-UKF model is weaker than the CS-UKF model. However, the RMSE of position predicted by the CS-UKF model is significantly greater than that of the CS-CKF model when CS is used as the motion model. It can be seen that, when the CS motion model is combined with the CKF trajectory prediction method, the RMSE of the vehicle position at the initial moment and after stabilization is relatively small. Figure 7b,c shows the RMSEs of predicted velocity and acceleration, respectively. From the numerical distribution, the error of CS-UKF and CS-CKF is smaller than that of CA-UKF. Judging from the range of data variation, the overall stability of CS-UKF and CS-CKF is slightly better than that of CA-UKF model. Therefore, the CS model has superior prediction accuracy and stability than the CA model when predicting the vehicle trajectory because "varying acceleration" is more suitable for the real state of vehicle motion. When the CS model is used as the basis of prediction model, the errors of the two models are similar. Figure 7d and the variation of the mean motion target RMSE in Table 1 show that the combined CS-CKF model outperforms the other two models in predicting the three performance metrics of position, velocity, and acceleration. Therefore, the CS-CKF model has superior estimation accuracy in vehicle trajectory prediction. In accordance with the introduction of CS model and CKF algorithm in the previous paper, the analysis shows that the change in vehicle acceleration is considered in the prediction process of CS model, which is closer to the actual situation. The CKF algorithm does not cause errors due to ignoring some nonlinear characteristics of the function itself, nor does it cause greater errors due to improper selection of parameters because it adopts the principle of cubature numerical integration. However, the system itself will have some errors when predicting the target. Thus, errors are always found in the final prediction results. The accuracy of vehicle positioning can be improved by using the CS-CKF model to a certain extent.

Conclusions
In the existing trajectory prediction models, the vehicle motion state is modelled as uniform linear motion or uniform acceleration to facilitate calculation, and the influence of vehicle acceleration change on trajectory prediction results is ignored. In this paper, the proposed model takes into account the fact that the acceleration of motor vehicles is constantly changing due to environmental factors during driving. This analysis is more in line with the current road conditions, where connected vehicles and human-driven vehicles are mixed. After that, we propose a CS-CKF trajectory prediction model to estimate the position, velocity, and acceleration of the vehicle in the process of motion. The prediction errors of CA-UKF, CS-UKF, and CS-CKF models are compared and analysed through the simulation of the actual trajectory of the intersection. The simulation results show that the CS-CKF model has superior accuracy, which ensures the stability in trajectory prediction.