Abstract
The obstacle motion state estimation is an essential task in intelligent vehicle. The ASCL group has developed such a system that uses a radar and GPS/INS. When running on the road, the acceleration of the vehicle is always changing, so it is hard for constant velocity (CV) model and constant acceleration (CA) model to describe the motion state of the vehicle. This paper introduced Current Statistical (CS) model from military field, which uses the modified Rayleigh distribution to describe acceleration. The adaptive Kalman filter based on CS model was used to estimate the motion state of the target. We conducted simulation experiments and real vehicle tests, and the results showed that the estimation of position, velocity, and acceleration can be precise.
1. Introduction
In recent years, the lane departure warning system, front collision warning system, adaptive cruise control system, and other automotive advanced driver assistance systems, which are based on the radar and computer vision, have become hot research topics in the international automotive safety technology [1–4].
In order to obtain the motion state information of the front vehicle comprehensively and achieve target identification effectively and accurately, the researchers of Bosch, Delphi, and other institutions proposed obstacle motion state estimation algorithm [5, 6]. And most of the studies use maneuvering target tracking methods from the military field, which are used to detect the aircraft, missile, and other flyers [7–11].
Friedland assumed that the target maintains a constant velocity relative to the radar, and the relative acceleration is considered to be the random interference in which the mean is zero. He established a two-order Kalman filter based on CV model [12]. The position and velocity estimation error are small when the velocity of the target is constant, but these estimation errors are larger when speed of the target changes.
Hampton assumed that the target maintains a constant acceleration relative to the radar, and the change of the relative acceleration is considered to be the random interference in which the mean is zero. He established a three-order Kalman filter based on CA model [13]. The position and velocity estimation error are small when the velocity or the acceleration of the target is constant, but these estimation errors are larger when the acceleration changes.
Singer proposed Singer model [14]. Singer model is a zero-mean model for motor acceleration, which is not reasonable to maneuvering target with mutative velocity and acceleration. It is generally agreed that the data range of next moment acceleration is limited.
Blom and Bar-Shalom proposed the Interactive Multiple Model (IMM) algorithm [15–18]. It allows for several parallel models which are combined to a weighted estimate. Choosing models for different driving modes, such as constant velocity, constant acceleration, and strong acceleration changes, the target state estimation can be optimized for highly dynamic maneuvers. But the amount of processing is large and if the weight set is not accurate enough, the estimation performance will decrease.
Hou of Tsinghua University assumed that the change rate of the acceleration is constant and he established a four-order Kalman filter [19]. When the acceleration of the target changes often, the position, velocity, and acceleration estimation is more accurate compared to the Kalman filter based on CV model or CA model. But it is a prior model and it is not an adaptive system according to the change of the acceleration. And if the change rate of the acceleration changes, the position, velocity, and acceleration estimation will be less precise.
In order to improve the accuracy of target state estimation, this paper introduces CS model [20, 21] from military and aerospace field, which is using the modified Rayleigh distribution to describe acceleration. Compared to the CA model, the CS model is more aligned with the actual acceleration change laws. The author established the adaptive Kalman filter based on the CS model. It uses the variance of the modified Rayleigh distribution to adjust the optimal Kalman gain at the next time, which improved the accuracy of the target motion state estimation. In addition, the motion modeling and filtering are based on absolute motion in absolute coordinate by using the GPS/INS, which can improve the accuracy.
The paper is organized as follows. In Section 1, a brief introduction on the target state estimation and modeling method is given. Section 2 presents the method for the target state estimation. In Sections 3 and 4 the simulation experiments and real vehicle test results are compared for CA model. Conclusions are presented in Section 5.
2. Target Motion State Estimation Model
In this paper the object state estimation is to use the estimation algorithm based on the target motion model to identify the state (position, velocity, and acceleration) accurately and in real time. The key element is to find a suitable model for target and an appropriate estimation algorithm.
When driving on the road, the time that the vehicle maintains constant velocity and acceleration is always short and the acceleration changes a lot, such as overtaking. So when using the Kalman filtering algorithm based on the CV model or the CA model to estimate the state of real vehicle, the error is large. But that of the adaptive Kalman filtering algorithm based on the CS model is not.
2.1. Target Motion Model
2.1.1. The “Current” Probability Density Model of the Maneuvering Target
Probability density function of Rayleigh distribution is expressed as follows:
Figure 1 shows that if is determined, we can completely determine the statistical properties of Rayleigh distribution.
When a target is maneuvering with a certain acceleration at present, the region of acceleration which can be taken in the next instant is limited and is around the “current” acceleration. Hence, it is unnecessary to take all possible values of maneuvering acceleration into consideration when modeling the target acceleration probability density. In this situation, the reasonable statistical model is a maneuvering acceleration probability distribution function that is time-varying and using the current acceleration of the target as the center, which is the modified Rayleigh distribution function as shown in Figure 2.
(1) When the acceleration of the obstacle is positive, the probability density function of the acceleration can be written as follows:where is positive maximum value of acceleration that the target can reach, is the acceleration of the target, and is a positive constant.
Mean value and variance of are given as follows:
(2) When the acceleration of the obstacle is negative, the probability density function of the acceleration can be written as follows:where is negative maximum value of acceleration that the target can reach, is the acceleration of the target, and is a positive constant.
Mean value and variance of are given as follows:
(3) When the acceleration of the obstacle is zero, the probability density function of the acceleration can be written as follows:where is the Dirac delta function.
In general, the maximum value of the target acceleration can be predicted. So when the mean value of the target acceleration is measured, can be written as follows:
And the variance of the acceleration is
Therefore, as long as the mean value of the target acceleration is obtained, the variance of the target acceleration can be obtained. The modified Rayleigh distribution is introduced to establish an appropriate relation between the mean and variance of the acceleration, so that the adaptive filtering algorithm is easy to implement.
2.1.2. Time-Correlation Model of Acceleration
Under the concept of “current” statistical model, when the target is maneuvering at a certain acceleration, the zero-mean model is obviously unreasonable, so the non-zero-mean time-correlation model of the acceleration can be used:where is the position, is the acceleration that the variance is and mean is , is the colored noise that the mean value is zero, is the maneuvering efficiency, is the white noise that the mean value is zero, and the variance is .
Let , and from (7) and (8), the following formulae can be deduced:where is the acceleration state variable and is the white noise in which the mean is .
From the estimation theory, the optimal estimation of the acceleration state variable is the conditional mean of the whole historical observed value :
And the variance of is
2.1.3. The Discrete State Equations
From (10), the following formula can be deduced:
Let be the sampling time and through the discrete processing, the discrete system state equation iswhere .
The state transition matrix iswhere is the sampling step.
The input matrix iswhere is the mean value of acceleration: is the process noise, which is the discrete-time white noise sequence that the covariance matrix is , is the acceleration variance in direction of the target, and is the acceleration variance in direction of the target:wherewhere is the autocorrelation time constant.
Equation (20) shows the self-adaptive adjusting method of modified Rayleigh distribution variance, which can reflect the change law of acceleration:
The observation equation iswhere is the observation noise which is the Gaussian white noise.
2.2. The Target Motion State Estimation Based on the Adaptive Kalman Filter [22]
(1) The initial values of the state and estimate covariance:
(2) Updated state estimate:
(3) Optimal Kalman gain:
(4) Updated state estimate:
(5) Updated estimate covariance:
(6) Predicted state estimate:
(7) The acceleration variances and use (17) and (18) and uses (16).
(8) Predicted estimate covariance:
(9) Return to (2) and continue the iteration.
When the mean of the modified Rayleigh distribution is small, the next time the acceleration value is in a wide range, which implies that the change of can be larger, so the Kalman gain should be larger to make the accuracy of filtering results not decline. Conversely, when the mean of the modified Rayleigh distribution is large, the next time the acceleration value is in a small range, which implies that the change of can be smaller, so the Kalman gain should be smaller.
From (18) and (20) it can be found that when the acceleration limit values and are determined and if the acceleration of the target changes in a small range, especially around zero, the tracking performance changes with the change of the process noise covariance matrix . Therefore, in order to ensure the tracking accuracy, the acceleration limit values and cannot be very large. And if and are too small, when the acceleration of target exceeds the range, the filtering performance will be worse. So the value of and cannot be too small or large.
3. Simulation
In tracking the status of target with different motions, there are different measurement results when using two different methods, the Kalman filter based on the CA model and the adaptive Kalman filter based on the CS model. When driving on the road, there are three kinds of motions, uniform motion, uniform acceleration motion, and varying acceleration motion, so these three simulation scenarios are selected. And we compare the two measurement results as follows.
Figure 3 shows the Gaussian white noise added in and direction in each simulation, in which the mean is 0 and the variance is 1.
Sampling step is 0.1 s; sampling time is 40 s.
3.1. Uniform Moving Target
The position, velocity, and acceleration estimation and the estimation error of uniform moving target are as follows.
Figure 4 shows the path of the uniform moving target, including the actual value, the results of Kalman filtering estimation based on CA model, and the results of adaptive Kalman filtering estimation based on CS model.
In Figure 5(a), the RMSE of the position of the CA model in direction is 0.1119 and the RMSE of the position of the CS model in direction is 0.1854; the RMSE error of the position of the CA model in direction is 0.1015 and the RMSE of the position of the CS model in direction is 0.1212.
(a) Position estimation and position estimation error
(b) Velocity estimation and velocity estimation error
(c) Acceleration estimation and acceleration estimation error
In Figure 5(b), the RMSE of the velocity of the CA model in direction is 0.5429 and the RMSE of the velocity of the CS model in direction is 0.7292; the RMSE of the velocity of the CA model in direction is 0.0935 and the RMSE of the velocity of the CS model in direction is 0.0057.
In Figure 5(c), the RMSE of the acceleration of the CA model in direction is 0.8933 and the RMSE of the acceleration of the CS model in direction is 0.7017; the RMSE of the acceleration of the CA model in direction is 0.5534 and the RMSE of the acceleration of the CS model in direction is 0.0498.
Figure 6 shows the trends of adaptive parameters and .
3.2. Moving Target with Constant Acceleration
The acceleration of the target in direction is 3 m/s and in direction is 0.3 m/s.
The position, velocity, and acceleration estimation and the estimation error of moving target with constant acceleration are as follows.
Figure 7 shows the path of the moving target with constant acceleration, including the actual value, the results of Kalman filtering estimation based on CA model, and the results of adaptive Kalman filtering estimation based on CS model.
In Figure 8(a), the RMSE of the position of the CA model in direction is 0.1197 and the RMSE of the position of the CS model in direction is 0.1493; the RMSE of the position of the CA model in direction is 0.1090 and the RMSE of the position of the CS model in direction is 0.1062.
(a) Position estimation and position estimation error
(b) Velocity estimation and velocity estimation error
(c) Acceleration estimation and acceleration estimation error
In Figure 8(b), the RMSE of the velocity of the CA model in direction is 0.2899 and RMSE of the velocity of the CS model in direction is 0.3070; the RMSE of the velocity of the CA model in direction is 0.1324 and the RMSE of the velocity of the CS model in direction is 0.1222.
In Figure 8(c), the RMSE of the acceleration of the CA model in direction is 0.5742 and RMSE of the acceleration of the CS model in direction is 0.5503; the RMSE of the acceleration of the CA model in direction is 0.0948 and the RMSE of the acceleration of the CS model in direction is 0.0773.
Figure 9 shows the trends of adaptive parameters and .
3.3. Moving Target with Variable Acceleration
The position, velocity, and acceleration estimation and the estimation error of moving target with variable acceleration are as follows.
Figure 10 shows the path of the moving target with variable acceleration, including the actual value, the results of Kalman filtering estimation based on CA model, and the results of adaptive Kalman filtering estimation based on CS model.
In Figure 11(a), the RMSE of the position of the CA model in direction is 2.4554 and the RMSE of the position of the CS model in direction is 0.2102; the RMSE of the position of the CA model in direction is 1.9182 and the RMSE of the position of the CS model in direction is 0.6944.
(a) Position estimation and position estimation error
(b) Velocity estimation and velocity estimation error
(c) Acceleration estimation and acceleration estimation error
In Figure 11(b), the RMSE of the velocity of the CA model in direction is 4.1254 and RMSE of the velocity of the CS model in direction is 0.8365; the RMSE of the velocity of the CA model in direction is 3.1801 and the RMSE of the velocity of the CS model in direction is 1.5912.
In Figure 11(c), the RMSE of the acceleration of the CA model in direction is 2.2319 and RMSE of the acceleration of the CS model in direction is 1.5602; the RMSE of the acceleration of the CA model in direction is 1.0856 and the RMSE of the acceleration of the CS model in direction is 1.4816.
Table 1 shows the RMSE for CA and CS model in the three simulation conditions. Figure 12 shows the trends of adaptive parameters and .
Regarding comprehensive comparison of these results, we can conclude the following:(1)When the target does uniform motion or constant acceleration motion, the results of Kalman filtering estimation on position, velocity, and acceleration based on the CA model and the adaptive Kalman filtering estimation on position, velocity, and acceleration based on the CS model can quickly converge. But the filtering estimation results of the former are closer to the exact value. This is because of the fact that, in the Kalman filtering estimation process, when the system process noise covariance matrix is larger, the filtering estimation is more biased in favor of the predicted value, which may cause the estimation results to not follow the actual change; when the observation noise covariance matrix is larger, the filtering estimation is more inclined to the observations, in which the observation noise filtering effect will reduce. In terms of the adaptive Kalman filtering algorithm based on the CS model, when the acceleration limit values, and , are determined, if the acceleration of the target changes in a small range (especially near zero), the tracking performance becomes worse as the system process noise covariance matrix is larger, conversely higher tracking performance.(2)When the target does the varying acceleration motion, the results of the Kalman filtering estimation on position, velocity, and acceleration based on the CA model are divergent, but that of the adaptive Kalman filtering estimation based on CS model can quickly converge and the errors are smaller.
4. Real Vehicle Test
Figure 13 shows the intelligent vehicle platform (Audi Q5) that is equipped with the IBEO four-layer laser radar, GPS/INS system, and industrial computer. Figure 14 shows the target vehicle (Buick GL8) that is equipped with GPS/INS system.
We used the GPS/INS system equipped on the intelligent vehicle to measure the position and heading angle of the radar operating and the IBEO radar to measure the relative position between the target vehicle and the intelligent vehicle. The GPS/INS system equipped on target vehicle recorded the absolute motion states, which we take as the accurate motion state of the target vehicle. And the GPS/INS systems equipped on both vehicles are Span-Cpt system that uses the professional level OEMV technology in which the precision can reach 0.01 meters.
We compared the algorithm introduced in this paper with the Kalman filter based on CA model.
Sampling step is 0.1 s; sampling time is 55 s.
Sampling condition is as follows: the target vehicle moves in front of the intelligent vehicle.
Figure 15 shows the path of the target vehicle, including the actual value, the results of Kalman filtering estimation based on CA model, and the results of adaptive Kalman filtering estimation based on CS model.
In Figure 16(a), the RMSE of the position of the CA model in direction is 1.8468 and the RMSE of the position of the CS model in direction is 1.6330; the RMSE of the position of the CA model in direction is 0.1728 and the RMSE of the position of the CS model in direction is 0.1440.
(a) Position estimation and position estimation error
(b) Velocity estimation and velocity estimation error
(c) Acceleration estimation and acceleration estimation error
In Figure 16(b), the RMSE of the velocity of the CA model in direction is 3.8322 and RMSE of the velocity of the CS model in direction is 2.1996; the RMSE of the velocity of the CA model in direction is 0.4923 and the RMSE of the velocity of the CS model in direction is 0.2345.
In Figure 16(c), the RMSE of the acceleration of the CA model in direction is 2.9796 and RMSE of the acceleration of the CS model in direction is 1.3044; the RMSE of the acceleration of the CA model in direction is 0.2808 and the RMSE of the acceleration of the CS model in direction is 0.2153.
Table 2 shows the RMSE for CA and CS model in the real vehicle test. Figure 17 shows the trends of adaptive parameters and .
Figures 16(a), 16(b), and 16(c) show that the error of the position, velocity, and acceleration of the adaptive Kalman filter based on the CS model is smaller than that of the Kalman filter based on the CA model.
When the target runs on the road, the acceleration varies with time, which is as the varying acceleration motion in the simulation. Respectively, using Kalman filter based on CA model and adaptive Kalman filter based on CS model to estimate the position, velocity, and acceleration, the errors of the latter are smaller.
5. Conclusion
The purpose of this paper is to estimate the motion state of the maneuvering obstacle in front of the intelligent vehicle. We used adaptive Kalman filter based on CS model which can identify the target position, velocity, and acceleration in real time through the sensor data with observation noise. In this paper we built a vehicle experimental platform which consists of an intelligent vehicle equipped with the laser radar, GPS/INS, and so forth and a tracking target (obstacle vehicle) equipped with inertial navigation system. And in this paper, the motion modeling and filtering are based on absolute motion in absolute coordinate by using the GPS/INS, which can improve the accuracy by decreasing the influence from the moving measuring bases. The simulation and experiments showed that the adaptive Kalman filter based on CS model can accurately estimate the target motion state when the targets are doing the uniform motion, constant acceleration motion, or varying acceleration motion.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgment
This paper is supported by Program for Chang Jiang Scholars and Innovative Research Team in University (no. IRT0626), China. Thanks are due to them.