# Research on Target Tracking Algorithm Using Millimeter-Wave Radar on Curved Road

Authors: Shiping Song; Jian Wu; Sumin Zhang; Yunhang Liu; Shun Yang
Journal: Mathematical Problems in Engineering
 (2020)

---

## Abstract

Millimeter-wave radar has been widely used in intelligent vehicle target detection. However, there are three difficulties in radar-based target tracking in curves. First, there are massive data association calculations with poor accuracy. Second, the lane position relationship of target-vehicle cannot be identified accurately. Third, the target tracking algorithm has poor robustness and accuracy. A target tracking algorithm framework on curved road is proposed herein. The following four algorithms are applied to reduce data association calculations and improve accuracy. (1) The data rationality judgment method is employed to eliminate target measurement data outside the radar detection range. (2) Effective target life cycle rules are used to eliminate false targets and clutter. (3) Manhattan distance clustering algorithm is used to cluster multiple data into one. (4) The correspondence between the measurement data received by the radar and the target source is identified by the nearest neighbor (NN) data association. The following three algorithms aim to derive the position relationship between the ego-vehicle and the target-vehicles. (1) The lateral speed is obtained by estimating the state of motion of the ego-vehicle. (2) An algorithm for state compensation of target motion is presented by considering the yaw motion of the ego-vehicle. (3) A target lane relationship recognition model is built. The improved adaptive extended Kalman filter (IAEKF) is used to improve the target tracking robustness and accuracy. Finally, the vehicle test verifies that the algorithms proposed herein can accurately identify the lane position relationship. Experiments show that the framework has higher target tracking accuracy and lower computational time.


---

## Body

## 1. Introduction

In recent years, vehicles with advanced driver assistance system (ADAS) have helped reduce traffic accidents and become a research focus at home and abroad. In addition to the small volume, the millimeter-wave radar is easy to install and strongly adaptable to bad weather such as rainy, snowy, and foggy days and nights. As it has a lower price than that of lidar, it has gained a high market share [1].In the real driving environment, the on-board millimeter-wave radar is affected by the thermal noise of the sensor and the external environment. As a consequence, the target state information detected includes noise. Therefore, in the real road environment with clutter, to realize accurate data association is the first difficulty in target tracking.The radar target tracking algorithm gets the lateral distance between the target-vehicle and the ego-vehicle through the radial distance and azimuth information. Then, the ego-vehicle can identify the lane relationship with the target-vehicles. The low resolution of radar azimuth leads to the erroneous lateral distance. The ego-vehicle has different driving courses on a straight road and a curved road. That is, the on-board millimeter-wave radar has yaw motion when the ego-vehicle runs on the curved road. If the influence of the yaw motion of the ego-vehicle on the motion compensation of the target-vehicle is not considered, the lateral distance between the target-vehicle and the ego-vehicle will increase. The ego-vehicle will be confronted with the following problems when it tracks multiple targets on the curved road. When the ego-vehicle is running on the curved road, the long-distance target-vehicle in its lane is incorrectly identified as that in the adjacent lane, and, as a result, the ego-vehicle runs maybe at an accelerated speed. During the acceleration of the ego-vehicle on the curved road, as the target-vehicle may suddenly appear in the lane, the ego-vehicle will slow down suddenly. This phenomenon will degrade the driving safety and ride comfort of the vehicle. Therefore, the target motion compensation plays a critical role in identifying the positional relationship of lane where the target-vehicle runs. While the target-vehicles travel on a curved road, to accurately define the lane position relationship of target-vehicles represents the second difficulty in target tracking.During the driving process, the statistical parameters of the observation noise of the on-board millimeter-wave radar are often unknown and time-varying. The noise statistical estimator from the adaptive extended Kalman filter is often used to calculate and correct noise statistical parameters online. The accuracy of the existing adaptive extended Kalman filter in calculating the observation noise is sacrificed for the ensuing algorithm and, as a consequence, the accuracy of target tracking is reduced. Therefore, the accuracy of target tracking needs to be improved while ensuring the robustness of the target tracking algorithm.In essence, the data association process is to define the relationship between the measured information received by the sensor and the target source, which is the core and most important part of the target tracking system. At present, the probability data association (PDA), interactive multiple models-probabilistic data association (IMMMDA), joint probabilistic data association (JPDA) algorithm, and multiple hypothesis tracking (MHT) algorithm are frequently used in the radar target tracking process [2].In the PDA, it is believed that there is only one target in the detection range of the millimeter-wave radar and that the track of this target has been formed [3]. In the practical application of radar, there is a certain amount of clutter around the real target. In the PDA, it is believed that all clutter may come from a real target but with different probabilities [4, 5]. As the target motion state is uncertain, it is generally assumed that there are many possible motion modes for the target. The combination of the interactive multiple models algorithm and the probabilistic data association will produce the interactive multiple models-probabilistic data association (IMMPDA), which can track mobile targets in a clutter environment [6, 7]. Therefore, the PDA and IMMPDA are only applied to the data association of single target tracking.For the purpose of data association in the multitarget tracking process, the joint probabilistic data association (JPDA) was proposed based on the PDA algorithm. In the JPDA algorithm, the validated matrix of the JPDA algorithm is employed to confirm the complicated relationship between the measurement data and the multitarget tracking gate [8–10]. Three problems in the application of JPDA algorithm in engineering need to be addressed. First, a joint event refers to an exponential function of all target measurement data detected by the radar. In a clutter-intensive environment, a combination explosion will occur [11]. Second, the JPDA method reuses the target measurement data falling in the intersection of the tracking gate. As a result, the number of effective target measurements that fall within the tracking gate also increases, making the joint event complicated and the amount of computation increased [12]. Third, the target measurement data falling within the intersection of the tracking gate are used to update the track; as a result, the Kalman filter variance is increased, which further leads to an increased number of false alarms within the tracking gate and ultimately degraded data association [13, 14].Multi hypothesis tracking (MHT) is another method for data association in the process of multitarget tracking. The MHT algorithm can be finished in the following two steps: Step1: Assume that the new target measurement data received by the millimeter-wave radar in each sampling period are new targets, false alarms, or existing targets [15]. Step 2: MHT establishes multiple candidate hypotheses through a time sliding window with finite length and then associates multitarget data through hypothesis evaluation and management (deletion, clustering, etc.) [16]. The MHT algorithm has the following two defects. First, the number of joint hypotheses increases exponentially along with the increase of target and clutter. Second, The MHT algorithm must sequence new target hypotheses in every sampling period. Therefore, as the MHT algorithm will need large computing resources in millimeter-wave radar target tracking, it is hard to meet the needs of real-time requirement.In order to eliminate the influence of the yaw motion of the ego-vehicle on the radar measurement data during driving on the curved road, motion compensation is required for the original target information detected by the radar. In the literature [17, 18], assuming that the ego-vehicle and the target-vehicle are traveling on roads with the same curvature, the azimuth between them is equal to the accurately measured central angle of the latter for its position compensation. However, the motion compensation has two disadvantages in multitarget tracking. First, as the target-vehicle and the ego-vehicle are on different lanes in the multitarget tracking process, the assumption is not true. The motion compensation in the multitarget tracking cannot be met. Second, the millimeter-wave radar acts as a sensor to detect the relative radial distance, azimuth, and relative radial rate of the target-vehicle against the ego-vehicle. Therefore, the target measurement data output by the radar includes either the location information or the speed information of the target. The motion compensation should include either the position compensation or the velocity compensation of the target-vehicle.Given tracking of the on-board millimeter-wave radar is nonlinear, particle filter (PF) and extended Kalman filter are employed due to their nonlinearity [19]. PF represents the required posterior probability density function through a set of random samples with weights. The accuracy and efficiency of the PF algorithm partly depend on the number of particles used in the estimation [20]. As there are a large number of targets detected by the on-board millimeter-wave radar in each cycle, a lot of particles are needed in resampling when PF is used for target tracking. In engineering application, it is found that the PF algorithm bears a large amount of computation for multitarget tracking. Therefore, the extended Kalman filter is often used in target tracking in on-board millimeter-wave radar. Since the statistical parameters of the observation noise in on-board millimeter-wave radar are often unknown and time-varying, the adaptive extended Kalman filter (AEKF) algorithm is adopted to calculate the statistical parameters of the noise. First, it estimates and updates the state of the system by iteration of state prediction and state correction and then calculates and corrects the noise statistical parameters online by the noise statistic estimator [21]. For now, the AEKF algorithm with unbiased noise statistic estimator has been extensively applied. However, matrix subtraction in the unbiased noise statistic estimator has two disadvantages. First, it will lead to the loss of nonnegative definiteness of the covariance matrix of process noise; second, it will result in the loss of positive definiteness of the covariance matrix of measurement noise. As a consequence, the robustness of AEKF target tracking algorithm is weakened. The algorithm robustness mentioned herein is defined as free of failure that the parameters in the algorithm violate the constraints and then makes the algorithm fail to continue running.After the comparative analysis of common data association methods, the following two disadvantages are drawn. First, the emergence of new targets and the disappearance of the original effective targets are not taken into account. Second, before data association, false targets and clutter are not excluded. In this paper, the data rationality judgment method is employed to eliminate the target measurement data outside the radar detection range. Given that a false target comes out randomly, the real target is continuous in a period of time. In this paper, the life cycle of the target is calculated by recording and analyzing the matching of target measurement data in a continuous period of time. By analyzing the effectiveness of the target, the false target and clutter can be removed. The Manhattan distance clustering algorithm is used to cluster multiple target measurement data into one. The difficulty of data association is reduced by eliminating false targets and clutter in radar measurement data and clustering analysis of target measurement data. The nearest neighbor (NN) data association is used for data association in multitarget tracking. The NN data association will fall in the tracking gate, and the radar measurement data with the smallest statistical distance or the largest residual probability density will be taken as the data association object. This method is characterized by easy implementation and small computation.Based on the above analysis, this paper proposes position compensation and velocity compensation of the target-vehicle based on the yaw motion of the ego-vehicle running on the curved road. In order to get the lateral velocity during the yaw motion of the ego-vehicle directly from the on-board sensor, 2-degree-of-freedom (2-DOF) vehicle dynamics model is built and the Kalman filter is used to filter the yaw rate of the ego-vehicle, and then a state observer is utilized to estimate the lateral velocity [22].Given that the commonly used AEKF has poor robustness and accuracy, the IAEKF algorithm and the NN data association are deduced herein.Compared with previous researches, this paper has three significant features:(1)
Motion compensation is executed on radar measurement data.(2)
A lane relationship identification model with easy calculation is established.(3)
In this paper, after the analysis of the poor robustness and low precision of the commonly used adaptive extended Kalman filter algorithm, an improved one with good robustness and high precision is derived.The rest of this paper is organized as follows. Section2 introduces the overall target motion compensation and tracking scheme with millimeter-wave radar. In Section 3, the motion state of the ego-vehicle is estimated by establishing a 2-DOF vehicle dynamics model and using the Kalman filter. In Section 4, the data rationality judgment criteria, target validity test, target motion compensation, target clustering analysis, and lane relationship identification model are presented. Besides, the IAEKF algorithm and NN data association algorithm are explained and deduced. Methods to screen key targets are also introduced. In Section 5, the algorithm is validated, and its results are analyzed by establishing a real vehicle test platform. Section 6 presents the conclusions.


## 2. Overall Scheme for Target Tracking by Radar

As shown in Figure1(b), the ego-vehicle is parked in an open space. Figure 1(a) is the display interface of the target measurement data in the open space. As illustrated, in the open space without effective targets (vehicles, two-wheelers, etc.), there are also many target measurement data affected by the thermal noise of the ground and millimeter-wave radar body.Figure 1
Target measurement data in an open scene. (a) Target measurement data. (b) Open space.
(a)(b)A target tracking process route based on the millimeter-wave radar target measurement data is proposed herein, as shown in Figure2.Figure 2
Target tracking process of the millimeter-wave radar.In this paper, the data rationality judgment method is employed to eliminate the target measurement information outside the radar detection range. By analyzing the effectiveness of the target, the false target and clutter can be removed. The following two steps are adopted to achieve the motion compensation of the target on the curved road. First, the motion state information of ego-vehicle is filtered and its state estimated to obtain the vehicle's longitudinal velocity, lateral velocity, and yaw rate. Second, the target measurement of millimeter-wave radar is judged by the data rationality. On this basis and in combination with the longitudinal velocity, lateral velocity, and yaw rate of the ego-vehicle, the motion of the target data of millimeter-wave radar is compensated. Due to the interference of clutter, even if there is only one real target, there are more than one target measurement data, so clustering analysis of the target measurement is required. Based on the above, the lane relationship identification model is established to judge the positional relationship of the target-vehicle and then track multiple targets at different lane positions. The NN data association method based on the elliptical tracking gate is used to define the correspondence between the measurement information received by the sensor and the object source. An IAEKF algorithm is applied for target tracking to have an increased robustness and improved accuracy of target tracking.


## 3. Ego-Vehicle State Estimation

At present, the ADAS function is mainly applied in highways [23]. As most highways are generally straight or curved with large radius and the lateral acceleration at the center of gravity of the vehicle is small, it is proposed to observe the motion state of the vehicle with a Kalman filter designed on the basis of the 2-DOF vehicle dynamics model and the Kalman filter equation presented herein.The influence of the steering system is ignored in the 2-DOF vehicle dynamics model and the front wheel angle is directly taken as an input. After the effect of the suspension is ignored, the vehicle compartment is believed to only move parallel to the ground; that is, the displacement of the vehicle along thez-axis, the elevation angle around the y-axis, and the roll angle around the x-axis are all zero. It is also assumed that the speed of the vehicle along the x-axis is constant. Therefore, the vehicle has only two degrees of freedom in the lateral movement along the y-axis and the yaw motion around the z-axis. The lateral acceleration at the center of gravity of the vehicle ranges from 0 to 0.4 g, and the tire deflection characteristics are in the linear range. In addition, to establish the differential equation, the air resistance and the change of the tire lateral deviation and the tire back at the moment due to the changed load in left and right tires are disregarded. The vehicle will be simplified to a 2-DOF vehicle dynamics model [24], as shown in Figure 3.Figure 3
2-DOF vehicle dynamics model.xooyo is the geodetic coordinate system, and xvoyv is the vehicle coordinate system; oyv is the horizontal axis of the vehicle coordinate system, and oxv is the vertical axis of the vehicle coordinate system; δ is the front wheel angle, and α1and α2 are the front wheel side angle and the rear wheel side angle, respectively; Fy1is the lateral tire force on front tire, and Fy2 is the lateral tire force on rear tire; β is the slip angle at vehicle center of gravity, v is the total velocity at center of gravity of vehicle, vx is the longitudinal velocity at center of gravity vehicle, and vy is the lateral velocity at center of gravity vehicle; a is the distance from center of gravity to front tire, and b is the distance from center of gravity to rear tire.Based on the lateral force balance and the moment balance around the center of gravity, the following dynamics equation can be got:(1)M∗ay=Fy1+Fy2,Iz∗ϑ¨=a∗Fy1−b∗Fy2,where ay is the lateral acceleration, Iz is the yaw moment of inertia of vehicle, and ϑ is the yaw angle of the vehicle rotating around the z-axis, so the yaw rate of the vehicle rotating around the z-axis satisfies the following formula:(2)ωr=ϑ˙.By substituting the linear deflection characteristics of the tire into the above formula, the following can be obtained [25]:(3)ϑ¨=a2∗K1+b2∗K2Iz∗vx∗ϑ˙+a∗K1−b∗K2Iz∗β−a∗K1Iz∗δ,β˙=K1∗a−K2∗bM∗vx2−1∗ϑ˙+K1+k2M∗vx∗β−K1M∗vx∗δ,where K1is the front tire cornering stiffness and K2 is the rear tire cornering stiffness. Since the 2-DOF vehicle dynamics model ignores the influence of the steering system, the following can be obtained [26]:(4)δ=δsgisg,where δsg is the steering wheel angle and isg is the transmission ratio of the steering system; β is the slip angle at the vehicle center of gravity. The following can be got based on the theory of vehicles [27]:(5)β=vyvx.The equation of state for 2-DOF vehicle dynamics model is [28](6)x˙=ϑ˙β=A∗x+B∗u+G∗w=A∗ϑ˙β+B∗δsg+G∗w1w2.Measurement equation is(7)z=ϑ˙=C∗x+v=C∗ϑ˙β+v,where(8)A=a2∗K1Iz∗vxa∗K1−b∗K2IzK1∗a−K2∗bM∗vx2−1K1+K2M∗vx,B=a∗K1isg∗IzK1isg∗M∗vx,G=aIz−bIz1Mvx1Mvx,C=10,w1and w2are the system noise brought by the lateral wind, v is the observation noise of yaw rate sensor, and w1,w2, and v are uncorrelated zero mean value Gaussian white noise; A is a state transition matrix, B is a control matrix, G is a system interference matrix, and C is a measurement matrix.The main parameters of the ego-vehicle are shown in Table1.Table 1
Vehicle model parameters.
SymbolValue (units)Parameter descriptionM1800 (kg)Vehicle massIZ2550 (kg·m2)Moment of inertia around thez-axisa1.201 (m)Distance from centroid to front axleb1.769 (m)Distance from centroid to rear axleisg17.32 (—)Steering ratioK11.71 × 105 (N/rad)Cornering stiffness of front wheelK21.32 × 105 (N/rad)Cornering stiffness of rear wheelThe Kalman filter is a linear optimal filter and its core is the Kalman filter equation [24]. The Kalman filter equation includes the time update process (prediction) and measurement update process (correction). The time update process is to obtain an estimate of the prior state of the next moment from the known current time system state. The measurement update process combines the prior state estimation during the time update process with the measurement data of the sensor to get a posteriori estimation of the state variable. Figure 4 illustrates the Kalman filter equation used herein.Figure 4
Schematic diagram of Kalman filter equation.x^−k+1 is the state prediction value at k + 1, H is the observation matrix, P−k+1 is the prediction value of the error covariance matrix at k + 1, uk is the control variable at k, Kk is the extended Kalman gain at k, and x^k is the estimate value of the extended Kalman filter at k, in which I is a unit matrix; x^−k0 is the mathematical expectation of the state variable at the initial time, and p^−k0 is the error covariance of the state variable at the initial time.


## 4. Target Tracking Algorithm for Millimeter-Wave Radar

### 4.1. Data Rationality Judgment

In order to meet the distance requirements of adaptive cruise control (ACC) detection or autonomous emergency braking (AEB) braking, this paper makes data rationality judgment in the light of the current international standards “Intelligent transportation systems-Full speed range adaptive cruise control systems-Performance requirements and test procedures” and “Intelligent transportation systems-Forward vehicle collision warning systems-Performance requirements and test procedures.”Table2 shows the specific technical specifications of the on-board millimeter-wave radar provided by the millimeter-wave radar supplier. Its main technical parameters such as detection range, measurement accuracy, and resolution are usually provided in its specification. A 77 GHz millimeter-wave radar is used herein with open target measurement data provided by the supplier. There are long-range radar and medium-range radar working states, which are subject to the driving speed of the ego-vehicle. When the travel speed of ego-vehicle is greater than 80 km/h and the millimeter-wave radar is in the long-distance radar working mode, it achieves the farthest detection distance of 120 m and the angle detection range of around 30°. When the travel speed of the ego-vehicle is less than 80 km/h and the millimeter-wave radar is in the medium-distance radar working mode, it reaches the farthest detection distance of 100m and the angle detection range of around 50°.Table 2
Main technical parameters of the millimeter-wave radar.
ParameterLong-distance modeMedium-distance modeRanging1 to 120 (m)1 to 100 (m)Ranging resolution0.8 (m)0.3 (m)Angle±30 (°)±50 (o)Speed resolution0.75 (km/h)0.6 (km/h)Speed−50 to 55 (m/s)−50 to 55 (m/s)Remark: ①The horizontal angle range is negative when the target is on the left side of the radar and positive when it is on the right side. ②The relative speed is positive when the target is far from the radar and negative when it is close to the radar.In the process of target tracking, the target-vehicle is located in three lanes, so the maximum millimeter-wave radar detection angle is selected as the target measurement angle, that is,−50°≤θ≤50°.When the ego-vehicle is driving at a higher speed, the target-vehicle that suddenly stops or runs at a lower speed is very dangerous. Therefore, the relative radial speed of the target-vehicle relative to the ego-vehicle within the detection range of the radar is selected as the effective target, that is,−50m/s≤V≤50m/s.In order to make the full-speed range ACC system have a large clearance under the highest speed, the radial distance target within 100 meters is selected as the effective target, that is,1m≤R≤100m.


### 4.2. Target Validity Test

The target validity test is to distinguish the real target and clutter through recording and analyzing the target measurement data in a continuous period of time [29, 30]. Figure 5 shows the validity test of the target and decision flowchart. The target validity test mainly includes the following three steps.Figure 5
The validity test of the target and decision flowchart.First, the target measurement data of successiveN sampling periods are stored in the memory block.Second, the target measurement data of the last sampling period stored in the memory block are used to predict the motion state of the target in the current period.(9)xnn−1ynn−1vxnn−1vynn−1=10t0010t00100001xn−1yn−1vxn−1vyn−1.The subscriptsn−1 and n of the variables in the formula represent the previous sampling period and the current sampling period, respectively. nn−1 represents the predicted n sampling period using the radar measurement data of the n sampling period. x is the longitudinal distance, y is the lateral distance, vy is the lateral velocity, and vx is the longitudinal velocity.t is the sampling time.Third, using formula (10), it should judge the data consistency between the target measured value transmitted after the rationality judgment of the data and the predicted value of the target of the previous cycle.(10)xnn−1ynn−1vxnn−1vynn−1−xnynvxnvyn≤kxkykvxkvy,where k is the threshold value, where the subscript represents the corresponding physical variable.If the predicted value of thenth sampling period and the measured value of the nth sampling period satisfy formula (10), the measured value of the nth sampling period is considered a valid value within this period, so the target motion state can be directly updated. If the predicted value of the nth sampling period and the measured value of the nth sampling period do not satisfy formula (10), the measured value of the nth sampling period and the target motion status which is in multiple sampling periods in the storage area should carry out a consistency judgment.We bring in the method of effective target life cycle to express the generation process of effective targets. The parameters used to describe the effective target life cycle are as follows: FindTime and LostTime.FindTime represents the number of times the measured value of thenth sampling period is consistent with the target motion status which is in multiple sampling periods. LostTime represents the number of times the measured value of the nth sampling period is inconsistent with the target motion status, which is in multiple sampling periods.Setting FindTime and LostTime has the following two functions:(1)
FindTime can rule out jamming targets that may be detected in a short period due to the unstable operation of the radar itself.(2)
LostTime can eliminate the temporary loss of the real target in a short period due to the random bumps and swings of the vehicle in the driving process.Q and H in Figure 5 represent the target motion state consistency threshold and the target motion state inconsistency threshold, respectively. The values in the text are 6 and 10, respectively.


### 4.3. Target Motion Compensation

It can be learned from the dynamics that the description of the same target motion state varies in different reference frames. It is therefore meaningful to clarify the coordinate system that describes the target motion.The three coordinate systems here are, respectively, the geodetic coordinate systemxoyozo, on the horizontal ground, the vehicle motion coordinate system xvyvzvwith its origin coinciding with the center of gravity vehicle, and the millimeter-wave radar coordinate system xroyr, as shown in Figure 6. The millimeter-wave radar is fixedly mounted on the front of the vehicle and the radar beam is aligned with the longitudinal axis of the vehicle. Therefore, the millimeter-wave radar coordinate system xroyr is parallel to the vehicle motion coordinate system xvyvzv. The radar xr axis direction is identical with the vertical axis xvdirection in the vehicle motion coordinate system. The radar yr axis direction is identical with the yv direction in the vehicle motion coordinate system.Figure 6
Coordinate system.The basic idea of target position compensation is to convert the target historical state information from the millimeter-wave radar coordinate system to the current millimeter-wave radar coordinate system. The target history state information refers to the position of the radar measurement data at the previous moment in the radar coordinate system at the current time. The target position compensation helps improve the accuracy of data association during target tracking in millimeter-wave radar.As shown in Figure7, in order to better explain the historical state information and the motion compensation principle, the process of motion compensation is illustrated by the translational motion of the ego-vehicle. It is assumed that there is a stationary target-vehicle A in front of the radar, and the ego-vehicle performs a translational motion in the XY plane. As shown in Figure 7(a), at time T = (k − 1) ∗ t, the ego-vehicle moves to point O, and the value of the radar measurement target-vehicle A is rk−1,φk−1. As shown in Figure 7(b), at the time T = (k) ∗ t, the ego-vehicle moves to the point O′, and the value of the radar measurement target-vehicle A is rk,φk. As shown in Figure 7(c), M′ refers to the position of target-vehicle A’s radar value measured at T = (k − 1) ∗ t in the radar coordinate system at T = (k) ∗ t. If the motion compensation of the vehicle is not considered, the target-vehicle A moves from M′ to M from T = (k − 1) ∗ t to T = (k) ∗ t. However, for a stationary target-vehicle A, the resulting displacement should be zero. The target history state information here refers to the position of target-vehicle A’s radar value [measured at T = (k − 1) ∗ t] M′ in the radar coordinate system at T = (k) ∗ t. Obviously, the target motion compensation is very important.Figure 7
An example of target-vehicle motion compensation. (a)T = (k − 1) ∗ t. (b) T = (k) ∗ t. (c) T = (k) ∗ t.
(a)(b)(c)The principle of target position compensation is shown in Figure8(a). The sampling time of millimeter-wave radar is t, and there is a target M in the radar beam plane XY. At T = (k − 1) ∗ t, the measurement of the target M position obtained by millimeter-wave radar isrk−1,φk−1. At T=(k − 1) ∗ t, the ego-vehicle’s lateral velocity vyk−1, longitudinal velocity vxk−1, and yaw rate ωk−1 can be obtained by the vehicle motion state estimation.Figure 8
Principle of target motion compensation. (a) Principle of position compensation. (b) Speed of position compensation.
(a)(b)The position of the historical position atT = (k − 1) ∗ t is expressed by formula (11) as the position at T = (k) ∗ t in the millimeter-wave radar coordinate system.(11)rkk−1=xk−1−xvk2+⋯+yk−1−yvk21/2,(12)φk|k−1=arctanyk−1−yvkxk−1−xvk+θk,where(13)xk−1=γk−1∗cosφk−1,yk−1=γk−1∗sinφk−1,xvk=vxk−1∗t,yvk=vyk−1∗t,θk=ωk−1∗t,where rkk−1 and φkk−1 are the relative radial distance and the azimuth in the radar polar coordinate system at T = (k) ∗ t, respectively. The relative radial distance and azimuth are measured at T = (k − 1) ∗ t. rk−1 and φk−1 are the relative radial distance and the azimuth of the target in the millimeter-wave radar polar coordinate system at T = (k − 1) ∗ t, respectively; xk−1and yk−1 are the longitudinal and lateral distances of the target in the millimeter-wave radar Cartesian coordinate system at T = (k − 1) ∗ t, respectively; xvk, yvk, and θkrepresent the longitudinal displacement, lateral displacement, and yaw angle of the ego-vehicle from T = (k − 1) ∗ t to T = (k) ∗ t, respectively. vxk−1, vyk−1, and ωk−1 are the longitudinal velocity, the lateral velocity, and the yaw rate of the ego-vehicle at T = (k − 1) ∗ t.The principle of target speed compensation is shown in Figure8(b), and there is a target M in the radar beam plane XY. The target M moves from M(k − 1) to the position M(k) as T changes from T = (k − 1) ∗ t to T = (k) ∗ t. At T = (k − 1) ∗ t, the millimeter-wave radar obtains the measurement of the target M as [r(k − 1), φ(k − 1)]. At T = (k) ∗ t, it obtains the measurement of the target M as [r(k), φ(k)].According to formulas (11) and (12), the historical position of the target at T = (k − 1) ∗ t can be expressed as follows in the millimeter-wave radar Cartesian coordinate system at T = (k) ∗ t:(14)xkk−1=γkk−1∗cosφkk−1,ykk−1=γkk−1∗sinφkk−1,where xkk−1and ykk−1represent the relative longitudinal distance and the relative lateral distance in the radar Cartesian coordinate system at T = (k) ∗ t, respectively. The relative longitudinal distance and the relative lateral distance are measured at T = (k − 1) ∗ t.The speed of the target in the millimeter-wave radar Cartesian coordinate system can be expressed as(15)vTar_rxk=xk−xkk−1t=vTar_xk−vxk−ωk∗rk∗sinφk,(16)vTar_ryk=yk−ykk−1t=vTar_yk−vyk+ωk∗rk∗cosφk,(17)xk=γk∗cosφk,(18)yk=γk∗sinφk,According to formulas (15) and (16), the actual speed of the target in the geodetic coordinate system is(19)vTar_xk=vTar_rxk+vxk+ωk∗rk∗sinφk,vTar_yk=vTar_ryk+vyk−ωk∗rk∗cosφk,where vTar_rxk and vTar_ryk represent the longitudinal and lateral velocities of the target in the millimeter-wave radar coordinate system at T = (k) ∗ t, respectively; vTar_xk and vTar_yk represent the longitudinal and lateral velocities of the target in the geodetic coordinate system at T = (k) ∗ t, respectively; [r(k), φ(k)] is the measurement data of the target M obtained by the millimeter-wave radar at T = (k) ∗ t.


### 4.4. Target Clustering

Due to the interference of clutter, even if there is only one real target, there may be more than one target measurement data. In order to cluster multiple target measurements into one target, targets clustering is done in this paper through Manhattan distance. The Manhattan distance formula for clustering at two points is(20)xi−xj<Threshol_dx,yi−yj<Threshol_dy,VTar_rx_i−VTar_rx_j<Threshol_Vx,VTar_ry_i−VTar_ry_j<Threshol_Vy,where Threshol_dx is the longitudinal distance clustering threshold, Threshol_dyis the lateral distance clustering threshold, Threshol_Vx is the longitudinal velocity clustering threshold, and Threshol_Vy is the lateral velocity clustering threshold.Within the detection range of the millimeter-wave radar, it is assumed that there are two points. The distances between the detected points and millimeter-wave radar arexi,yi andxj,yj, respectively, and the speed VTarrxi,VTarryi and VTarrxj,VTarryj, respectively.


### 4.5. Lane Relationship Identification Model

To identify the positional relationship between the target-vehicle and the ego-vehicle, the lateral displacement between the target-vehicle and the ego-vehicle needs to be studied. When the ego-vehicle and the target-vehicle travel on a curved road, the lateral displacement between the target-vehicle and the ego-vehicle is greatly subject to road curvature. Therefore, the identification of road curvature is a critical parameter for judging the positional relationship of the target.Generally, the curvature of the road where the ego-vehicle travels on can be predicted by its speed, yaw rate, steering wheel angle, and total wheel base. As the tire’s yaw angle will cause the steering center to change when the vehicle travels at high speed, the prediction of road curvature where the ego-vehicle travels at the steering wheel angle only applies to the low speed condition. Since the prediction of the driving trajectory of the ego-vehicle is performed when the vehicle runs near the steady state, this method only applies to the fact that the vehicle has been stably traveling on a straight or curved road. When the vehicle is about to travel to or away from a bend or in case of other unsteady motion states, no accurate estimation can be done.Based on the above analysis, the following formula is used herein to estimate the road curvature of the ego-vehicle:(21)κ=ωvx,v>15km/h,δisgL,v≤15km/h,where L is the total wheel base.Since they have a similar principle of identifying the target lane relationship of the left and right curved roads, the symbols of the partial variables need to be modified only. Thus, the lane relationship identification model between the target-vehicle and the ego-vehicle on a curved road is illustrated by a right turn.Figure9 illustrates the right-turning lane relationship. The millimeter-wave radar is installed at the center point A of the front bumper of the ego-vehicle. It is assumed that the millimeter-wave radar detects the midpoint B of the target-vehicle and that the distance r between A and B is the relative radial distance of the millimeter-wave radar detection target. The angle φ between the A-B line and the medial axis of the ego-vehicle is the azimuth of the millimeter-wave radar detection target. When the target-vehicle is on the right of the medial axis of the ego-vehicle, the azimuth φ>0, and when the target-vehicle is on the left, the azimuth φ<0. W is the standard lane width, dL the distance between the ego-vehicle and the left lane line, and dR is the distance between the ego-vehicle and the right lane line. Rr is the radius of the lane on which the target-vehicle travels, and Ro is the radius of the lane on which the ego-vehicle travels. D is the lateral distance between the ego-vehicle and the target-vehicle.Figure 9
Right turn curve lane relationship identification.It can be concluded from the cosine theorem in trigonometric functions that(22)Rr2=Ro2+r2−2Ro∗r∗cos90°−φ.Therefore, the lateral distance between the target-vehicle and the ego-vehicle is(23)D=Rr−Ro.①
Identification model when the target-vehicle and the ego-vehicle are in the same lane is(24)D≤dR+12B+12M,D<0,D≥dL+12B+12M,D>0,whereB is the width of the ego-vehicle and M is the lane marking line width.②
Identification model when the target-vehicle travels on the adjacent lane on the left side of the ego-vehicle is(25)dL+12B+12M<D<dL+12B+12M+W.③
Identification model when the target-vehicle travels on the adjacent lane on the right side of the ego-vehicle is(26)dR+12B+12M<D<dR+12B+12M+W.


### 4.6. Target Tracking Based on Improved Adaptive Extended Kalman Filter

As the vehicle travels on the road, the millimeter-wave radar detects targets in front of the vehicle, mainly including vehicles and road test isolation belts. These targets have no vertical motion or small moving speed in the vertical direction, so only the movement of the target-vehicle in theXY plane needs to be considered. Since the target-vehicle motion state is characterized by small mobility, a constant acceleration (CA) model is established based on the millimeter-wave radar motion coordinate system to describe the motion state of the front target-vehicle.When depicting the target motion, the target is regarded as a point in space regardless of its shape. The state of motion of the target is described in line with the laws of Newton’s kinematics. The model of the target motion state is as follows:(27)Xk+1=A∗Xk+B∗wk.The target motion state vectorX=x,y,x˙,y˙,x¨,y¨T here is designed to describe the motion state of the target-vehicle in the millimeter-wave radar Cartesian coordinate system, where x,y, x˙,y˙, and x¨,y¨ represent the position, velocity, and acceleration of the target-vehicle in the millimeter-wave radar Cartesian coordinate system, respectively.A is the state transition matrix, B is noise-driven matrix, and wk is the process noise at k:(28)A=10t0t220010t0t220010t000010t000010000001,B=t2200t22t00t0000.According to the principle of millimeter-wave radar, the millimeter-wave radar measures the relative radial distanceR and the relative radial velocity V between the target-vehicle and the ego-vehicle. The target tracking here is based on the vehicle's millimeter-wave radar coordinate system. The position of the target at k is assumed to be xk,yk, with velocityxk˙,yk˙. The observation Z consists of an acceleration and the relative radial distance R, as well as the relative radial velocity V between the target-vehicle and the ego-vehicle, where the relative radial distance R and the relative radial velocity V are measured from the millimeter-wave radar emission and the reflected wave, and the acceleration is calculated based on the ratio of the speed difference between the front and rear time and the sampling time. The observation equation is(29)Zk=Z1k=Rk=x2k+y2k+ν1k,Z2k=Vk=x˙2k+y˙2k+ν2k,Z3k=axk=xk˙−xk−1˙t+ν3k,Z4k=ayk=yk˙−yk−1˙t+ν4k,where ax is the longitudinal acceleration, ay is lateral acceleration, and vik is observation noise on k; t is the sampling time.Equations (25) and (27) are the state equation and the observation equation, respectively. As illustrated in the system model, the state equation is linear and the observation equation is nonlinear. Since the observation equation is nonlinear, the target tracking system based on the millimeter-wave radar is nonlinear.Since the statistical parameters of the observation noise of on-board millimeter-wave radar are often unknown and time-varying, the adaptive extended Kalman filter algorithm is adopted to calculate the statistical parameters of the noise. First, it estimates and updates the state of the system by iteration of state prediction and state correction and then calculates and corrects the noise statistical parameters online by noise statistic estimator.The extended Kalman filter (EKF) is an optimal estimation method for extracting the required signal from the sensor measurement signals based on a recursive algorithm and in line with the linear unbiased and minimum variance criteria. It is suitable for solving problems by computer and needs less computation and storage, so it has been widely applied in engineering [31, 32]. Its implementation steps are as follows.Step 1.
State prediction(30)x^kk−1=fx^k−1,uk−1,0,Pkk−1=Ak∗Pk−1∗ATk+Qk.Step 2.
State correction(31)Skk−1=Hk∗Pkk−1∗HTk+Rk,(32)Κk=Pkk−1∗HTk∗S−1kk−1,(33)x^k=x^k|k−1+Kk∗zk−z^kk−1,(34)PK=Pkk−1−Kk∗Hk∗Pkk−1.
For now, the adaptive extended Kalman filter algorithm with unbiased noise statistic estimator has been broadly applied. For example, the Sage extended Kalman filter (Sage-EKF) algorithm is used to estimate the observation noise and the system noise covariance matrix from residual sequence, so that the target motion state estimation is got from [33]. The unbiased noise statistic estimator includes the following steps:(35)εk=zk−z^kk−1,where z^kk−1=Hk∗x^kk−1.(36)dk=1−b1−bk+1,Rk+1=1−dkRk+dkεk∗εTk−Hk∗Pkk−1∗HTK,Qk+1=1−dkQk+dkKkεkεTkKTk+Pk−AkPk−1ATk,where εk is the innovation, dk is the forgetting factor at k, and b is the forgetting factor within the range of [0.95, 0.995]; b is 0.97 here.
However, matrix subtraction in the unbiased noise statistic estimator will lead to the loss of nonnegative definiteness of the covariance matrix of process noiseQ or the loss of positive definiteness of the covariance matrix of measurement noise R, thus weakening the robustness of the adaptive extended Kalman filter target tracking algorithm. Therefore, an improved adaptive extended Kalman filter algorithm, combining the unbiased noise statistic estimator and biased noise statistic estimator, is advanced in [34] to ensure the robustness of the adaptive extended Kalman filter algorithm. Its implementation steps are as follows:(37)Rk+1=Rk+1,ifRk+1is positive definite,1−dk∗Rk+dkεk∗εTk,otherwise,(38)Qk+1=Qk+1,ifQk+1is positive definite,1−dkQk+dkKkεkεTkKkT,otherwise.
Formulas (37) and (38) are biased noise estimators. They have the following two problems. On the one hand, the process noise covariance matrix Q discards some of the state estimation error covariance terms. On the other hand, the measurement noise covariance matrix R discards some of the measurement error covariance terms.
Since the discarded covariance term can correct the unbiased noise statistic estimator, the covariance matrix of process noiseQ and the covariance matrix of measurement noise R are inaccurate, thus reducing the accuracy of the adaptive extended Kalman filter. Based on [35], a new fault tolerant noise statistic estimator is proposed. The biased noise statistic estimator in [35] is as follows:(39)Rk+1=Rk+1,ifRk+1is positive definite,1−dk∗Rk+dkdiagzk−z^kk−12+HkPkHTk,otherwise,(40)Qk+1=Qk+1,ifQk+1is positive definite,1−dkQk+dkdiagx^k−x^kk−12−Pkk−1+Qk,otherwise.
The diag(■) in formulas (39) and (40) represents a diagonal matrix of the same dimension composed of diagonal elements of the operation matrix.
According to formula (34),(41)PK−Pkk−1=−Kk∗Hk∗Pkk−1.
By substituting formula (41) into (40), the following can be got:(42)Qk+1=Qk+1,ifQk+1is positive definite,1−dkQk+dkdiagx^k−x^kk−12+Kk∗Hk∗Pkk−1−Qk,otherwise,
Since the matrix subtraction may cause the process noise covarianceQ to lose nonnegative definiteness, the negative Q is rejected and the following can be obtained:(43)Qk+1=Qk+1,ifQk+1ispositive definite,1−dkQk+dkdiagx^k−x^kk−12+Kk∗Hk∗Pkk−1,otherwise.
According to formula (32),(44)Hk∗Pkk−1=Skk−1∗KTk.
By substituting formula (44) into (43), the following can be obtained:(45)Qk+1=Qk+1,ifQk+1is positive definite,1−dkQk+dkdiagx^k−x^kk−12+Kk∗Skk−1∗KTk,otherwise.
According to formulas (33) and (35),(46)x^k−x^kk−12=Kk∗εk∗εTk∗KTk.
By substituting formula (46) into (45), the following can be obtained:(47)Qk+1=Qk+1,ifQk+1is positive definite,1−dkQk+dkdiagKkεkεTkΚk+⋯+KkSk|k−1KTk,otherwise.
By substituting formula (37) into (35), the following can be obtained:(48)Rk+1=Rk+1,ifRk+1is positive  definite,1−dk∗Rk+dkdiagεkεTk+⋯+HkPkHTk,otherwise.
Formulas (47) and (48) present the new fault tolerant noise statistic estimator proposed herein. The new fault tolerant noise statistic estimator keeps the covariance terms from most unbiased noise statistic estimators and can improve the accuracy of the algorithm. As it can ensure that the covariance matrix of measurement noise R is always of positive definiteness and the covariance matrix of process noise Q is always of nonnegative definiteness, it delivers an improved robustness of the algorithm.
The implementation of the target tracking algorithm is based on the correct data association. If the data association is wrong, the motion state information obtained after the estimation will be of poor quality. Therefore, it is required to select a data association method with probability statistics to establish a correspondence between the target and the measurement.
A tracking gate is required for the data association algorithm to define the possible areas of measurement from the target by centering on the forecast position of the target [36]. The tracking gate has the following three functions: ① Determine the candidate measurement: when the measurement falls into the tracking gate area, it will be used to update the tracked target state. ② Establish a new target motion track: when the measurement does not fall into the tracking gate area, it will be taken as a new target or the existing interference clutter. ③ Confirm that the target is lost: when there is no measurement in the tracking gate for several continuing periods, the target will be considered lost.
The size and shape of the tracking gate play an important role. Its size depends on the probability of receiving the correct echo. It is generally rectangular or elliptical. Generally, the elliptic tracking gate is more widely used in engineering than the rectangular one. Hence, the elliptic tracking gate is adopted herein.
An NN data association method is proposed herein and is characterized by convenient implementation and a small amount of computation. Among the plurality of target measurement data within the tracking gate, the one closest to the predicted position is selected as the associated object of the target.
In the above adaptive extended Kalman filter algorithm, formula (35) gives the difference εk between the system observation value zk at the current moment k and the observation value z^kk−1 of the system state prediction at the previous moment, that is, the innovation.
The covariance matrix of the innovation is(49)Sk=EεkεTk=Ezk−z^kk−1zk−z^kk−1T.
The following can be got:(50)Sk=HkPkk−1HTk+Rk.
The statistical distance is defined as the weighted norm of the innovation vector:(51)d2k=εTkS−1kεk.
When the distance between the adjacent measurement vectors of the same targetd2k is less than a certain value λ, the target is considered to fall into the tracking gate, and the λ size may be calibrated according to the speed, distance, and the like of the object.(52)d2k≤λ.
As shown in Figure10, it is assumed that there is a track M and that the associated gate used for the forecast position is elliptical. There are observation points a, b, c, and d of the sensor near the elliptical tracking gate, where the point d is outside the associated gate, points a, b, and c are within the associated gate, and point c is the NN.Figure 10
NN data association.


## 4.1. Data Rationality Judgment

In order to meet the distance requirements of adaptive cruise control (ACC) detection or autonomous emergency braking (AEB) braking, this paper makes data rationality judgment in the light of the current international standards “Intelligent transportation systems-Full speed range adaptive cruise control systems-Performance requirements and test procedures” and “Intelligent transportation systems-Forward vehicle collision warning systems-Performance requirements and test procedures.”Table2 shows the specific technical specifications of the on-board millimeter-wave radar provided by the millimeter-wave radar supplier. Its main technical parameters such as detection range, measurement accuracy, and resolution are usually provided in its specification. A 77 GHz millimeter-wave radar is used herein with open target measurement data provided by the supplier. There are long-range radar and medium-range radar working states, which are subject to the driving speed of the ego-vehicle. When the travel speed of ego-vehicle is greater than 80 km/h and the millimeter-wave radar is in the long-distance radar working mode, it achieves the farthest detection distance of 120 m and the angle detection range of around 30°. When the travel speed of the ego-vehicle is less than 80 km/h and the millimeter-wave radar is in the medium-distance radar working mode, it reaches the farthest detection distance of 100m and the angle detection range of around 50°.Table 2
Main technical parameters of the millimeter-wave radar.
ParameterLong-distance modeMedium-distance modeRanging1 to 120 (m)1 to 100 (m)Ranging resolution0.8 (m)0.3 (m)Angle±30 (°)±50 (o)Speed resolution0.75 (km/h)0.6 (km/h)Speed−50 to 55 (m/s)−50 to 55 (m/s)Remark: ①The horizontal angle range is negative when the target is on the left side of the radar and positive when it is on the right side. ②The relative speed is positive when the target is far from the radar and negative when it is close to the radar.In the process of target tracking, the target-vehicle is located in three lanes, so the maximum millimeter-wave radar detection angle is selected as the target measurement angle, that is,−50°≤θ≤50°.When the ego-vehicle is driving at a higher speed, the target-vehicle that suddenly stops or runs at a lower speed is very dangerous. Therefore, the relative radial speed of the target-vehicle relative to the ego-vehicle within the detection range of the radar is selected as the effective target, that is,−50m/s≤V≤50m/s.In order to make the full-speed range ACC system have a large clearance under the highest speed, the radial distance target within 100 meters is selected as the effective target, that is,1m≤R≤100m.


## 4.2. Target Validity Test

The target validity test is to distinguish the real target and clutter through recording and analyzing the target measurement data in a continuous period of time [29, 30]. Figure 5 shows the validity test of the target and decision flowchart. The target validity test mainly includes the following three steps.Figure 5
The validity test of the target and decision flowchart.First, the target measurement data of successiveN sampling periods are stored in the memory block.Second, the target measurement data of the last sampling period stored in the memory block are used to predict the motion state of the target in the current period.(9)xnn−1ynn−1vxnn−1vynn−1=10t0010t00100001xn−1yn−1vxn−1vyn−1.The subscriptsn−1 and n of the variables in the formula represent the previous sampling period and the current sampling period, respectively. nn−1 represents the predicted n sampling period using the radar measurement data of the n sampling period. x is the longitudinal distance, y is the lateral distance, vy is the lateral velocity, and vx is the longitudinal velocity.t is the sampling time.Third, using formula (10), it should judge the data consistency between the target measured value transmitted after the rationality judgment of the data and the predicted value of the target of the previous cycle.(10)xnn−1ynn−1vxnn−1vynn−1−xnynvxnvyn≤kxkykvxkvy,where k is the threshold value, where the subscript represents the corresponding physical variable.If the predicted value of thenth sampling period and the measured value of the nth sampling period satisfy formula (10), the measured value of the nth sampling period is considered a valid value within this period, so the target motion state can be directly updated. If the predicted value of the nth sampling period and the measured value of the nth sampling period do not satisfy formula (10), the measured value of the nth sampling period and the target motion status which is in multiple sampling periods in the storage area should carry out a consistency judgment.We bring in the method of effective target life cycle to express the generation process of effective targets. The parameters used to describe the effective target life cycle are as follows: FindTime and LostTime.FindTime represents the number of times the measured value of thenth sampling period is consistent with the target motion status which is in multiple sampling periods. LostTime represents the number of times the measured value of the nth sampling period is inconsistent with the target motion status, which is in multiple sampling periods.Setting FindTime and LostTime has the following two functions:(1)
FindTime can rule out jamming targets that may be detected in a short period due to the unstable operation of the radar itself.(2)
LostTime can eliminate the temporary loss of the real target in a short period due to the random bumps and swings of the vehicle in the driving process.Q and H in Figure 5 represent the target motion state consistency threshold and the target motion state inconsistency threshold, respectively. The values in the text are 6 and 10, respectively.


## 4.3. Target Motion Compensation

It can be learned from the dynamics that the description of the same target motion state varies in different reference frames. It is therefore meaningful to clarify the coordinate system that describes the target motion.The three coordinate systems here are, respectively, the geodetic coordinate systemxoyozo, on the horizontal ground, the vehicle motion coordinate system xvyvzvwith its origin coinciding with the center of gravity vehicle, and the millimeter-wave radar coordinate system xroyr, as shown in Figure 6. The millimeter-wave radar is fixedly mounted on the front of the vehicle and the radar beam is aligned with the longitudinal axis of the vehicle. Therefore, the millimeter-wave radar coordinate system xroyr is parallel to the vehicle motion coordinate system xvyvzv. The radar xr axis direction is identical with the vertical axis xvdirection in the vehicle motion coordinate system. The radar yr axis direction is identical with the yv direction in the vehicle motion coordinate system.Figure 6
Coordinate system.The basic idea of target position compensation is to convert the target historical state information from the millimeter-wave radar coordinate system to the current millimeter-wave radar coordinate system. The target history state information refers to the position of the radar measurement data at the previous moment in the radar coordinate system at the current time. The target position compensation helps improve the accuracy of data association during target tracking in millimeter-wave radar.As shown in Figure7, in order to better explain the historical state information and the motion compensation principle, the process of motion compensation is illustrated by the translational motion of the ego-vehicle. It is assumed that there is a stationary target-vehicle A in front of the radar, and the ego-vehicle performs a translational motion in the XY plane. As shown in Figure 7(a), at time T = (k − 1) ∗ t, the ego-vehicle moves to point O, and the value of the radar measurement target-vehicle A is rk−1,φk−1. As shown in Figure 7(b), at the time T = (k) ∗ t, the ego-vehicle moves to the point O′, and the value of the radar measurement target-vehicle A is rk,φk. As shown in Figure 7(c), M′ refers to the position of target-vehicle A’s radar value measured at T = (k − 1) ∗ t in the radar coordinate system at T = (k) ∗ t. If the motion compensation of the vehicle is not considered, the target-vehicle A moves from M′ to M from T = (k − 1) ∗ t to T = (k) ∗ t. However, for a stationary target-vehicle A, the resulting displacement should be zero. The target history state information here refers to the position of target-vehicle A’s radar value [measured at T = (k − 1) ∗ t] M′ in the radar coordinate system at T = (k) ∗ t. Obviously, the target motion compensation is very important.Figure 7
An example of target-vehicle motion compensation. (a)T = (k − 1) ∗ t. (b) T = (k) ∗ t. (c) T = (k) ∗ t.
(a)(b)(c)The principle of target position compensation is shown in Figure8(a). The sampling time of millimeter-wave radar is t, and there is a target M in the radar beam plane XY. At T = (k − 1) ∗ t, the measurement of the target M position obtained by millimeter-wave radar isrk−1,φk−1. At T=(k − 1) ∗ t, the ego-vehicle’s lateral velocity vyk−1, longitudinal velocity vxk−1, and yaw rate ωk−1 can be obtained by the vehicle motion state estimation.Figure 8
Principle of target motion compensation. (a) Principle of position compensation. (b) Speed of position compensation.
(a)(b)The position of the historical position atT = (k − 1) ∗ t is expressed by formula (11) as the position at T = (k) ∗ t in the millimeter-wave radar coordinate system.(11)rkk−1=xk−1−xvk2+⋯+yk−1−yvk21/2,(12)φk|k−1=arctanyk−1−yvkxk−1−xvk+θk,where(13)xk−1=γk−1∗cosφk−1,yk−1=γk−1∗sinφk−1,xvk=vxk−1∗t,yvk=vyk−1∗t,θk=ωk−1∗t,where rkk−1 and φkk−1 are the relative radial distance and the azimuth in the radar polar coordinate system at T = (k) ∗ t, respectively. The relative radial distance and azimuth are measured at T = (k − 1) ∗ t. rk−1 and φk−1 are the relative radial distance and the azimuth of the target in the millimeter-wave radar polar coordinate system at T = (k − 1) ∗ t, respectively; xk−1and yk−1 are the longitudinal and lateral distances of the target in the millimeter-wave radar Cartesian coordinate system at T = (k − 1) ∗ t, respectively; xvk, yvk, and θkrepresent the longitudinal displacement, lateral displacement, and yaw angle of the ego-vehicle from T = (k − 1) ∗ t to T = (k) ∗ t, respectively. vxk−1, vyk−1, and ωk−1 are the longitudinal velocity, the lateral velocity, and the yaw rate of the ego-vehicle at T = (k − 1) ∗ t.The principle of target speed compensation is shown in Figure8(b), and there is a target M in the radar beam plane XY. The target M moves from M(k − 1) to the position M(k) as T changes from T = (k − 1) ∗ t to T = (k) ∗ t. At T = (k − 1) ∗ t, the millimeter-wave radar obtains the measurement of the target M as [r(k − 1), φ(k − 1)]. At T = (k) ∗ t, it obtains the measurement of the target M as [r(k), φ(k)].According to formulas (11) and (12), the historical position of the target at T = (k − 1) ∗ t can be expressed as follows in the millimeter-wave radar Cartesian coordinate system at T = (k) ∗ t:(14)xkk−1=γkk−1∗cosφkk−1,ykk−1=γkk−1∗sinφkk−1,where xkk−1and ykk−1represent the relative longitudinal distance and the relative lateral distance in the radar Cartesian coordinate system at T = (k) ∗ t, respectively. The relative longitudinal distance and the relative lateral distance are measured at T = (k − 1) ∗ t.The speed of the target in the millimeter-wave radar Cartesian coordinate system can be expressed as(15)vTar_rxk=xk−xkk−1t=vTar_xk−vxk−ωk∗rk∗sinφk,(16)vTar_ryk=yk−ykk−1t=vTar_yk−vyk+ωk∗rk∗cosφk,(17)xk=γk∗cosφk,(18)yk=γk∗sinφk,According to formulas (15) and (16), the actual speed of the target in the geodetic coordinate system is(19)vTar_xk=vTar_rxk+vxk+ωk∗rk∗sinφk,vTar_yk=vTar_ryk+vyk−ωk∗rk∗cosφk,where vTar_rxk and vTar_ryk represent the longitudinal and lateral velocities of the target in the millimeter-wave radar coordinate system at T = (k) ∗ t, respectively; vTar_xk and vTar_yk represent the longitudinal and lateral velocities of the target in the geodetic coordinate system at T = (k) ∗ t, respectively; [r(k), φ(k)] is the measurement data of the target M obtained by the millimeter-wave radar at T = (k) ∗ t.


## 4.4. Target Clustering

Due to the interference of clutter, even if there is only one real target, there may be more than one target measurement data. In order to cluster multiple target measurements into one target, targets clustering is done in this paper through Manhattan distance. The Manhattan distance formula for clustering at two points is(20)xi−xj<Threshol_dx,yi−yj<Threshol_dy,VTar_rx_i−VTar_rx_j<Threshol_Vx,VTar_ry_i−VTar_ry_j<Threshol_Vy,where Threshol_dx is the longitudinal distance clustering threshold, Threshol_dyis the lateral distance clustering threshold, Threshol_Vx is the longitudinal velocity clustering threshold, and Threshol_Vy is the lateral velocity clustering threshold.Within the detection range of the millimeter-wave radar, it is assumed that there are two points. The distances between the detected points and millimeter-wave radar arexi,yi andxj,yj, respectively, and the speed VTarrxi,VTarryi and VTarrxj,VTarryj, respectively.


## 4.5. Lane Relationship Identification Model

To identify the positional relationship between the target-vehicle and the ego-vehicle, the lateral displacement between the target-vehicle and the ego-vehicle needs to be studied. When the ego-vehicle and the target-vehicle travel on a curved road, the lateral displacement between the target-vehicle and the ego-vehicle is greatly subject to road curvature. Therefore, the identification of road curvature is a critical parameter for judging the positional relationship of the target.Generally, the curvature of the road where the ego-vehicle travels on can be predicted by its speed, yaw rate, steering wheel angle, and total wheel base. As the tire’s yaw angle will cause the steering center to change when the vehicle travels at high speed, the prediction of road curvature where the ego-vehicle travels at the steering wheel angle only applies to the low speed condition. Since the prediction of the driving trajectory of the ego-vehicle is performed when the vehicle runs near the steady state, this method only applies to the fact that the vehicle has been stably traveling on a straight or curved road. When the vehicle is about to travel to or away from a bend or in case of other unsteady motion states, no accurate estimation can be done.Based on the above analysis, the following formula is used herein to estimate the road curvature of the ego-vehicle:(21)κ=ωvx,v>15km/h,δisgL,v≤15km/h,where L is the total wheel base.Since they have a similar principle of identifying the target lane relationship of the left and right curved roads, the symbols of the partial variables need to be modified only. Thus, the lane relationship identification model between the target-vehicle and the ego-vehicle on a curved road is illustrated by a right turn.Figure9 illustrates the right-turning lane relationship. The millimeter-wave radar is installed at the center point A of the front bumper of the ego-vehicle. It is assumed that the millimeter-wave radar detects the midpoint B of the target-vehicle and that the distance r between A and B is the relative radial distance of the millimeter-wave radar detection target. The angle φ between the A-B line and the medial axis of the ego-vehicle is the azimuth of the millimeter-wave radar detection target. When the target-vehicle is on the right of the medial axis of the ego-vehicle, the azimuth φ>0, and when the target-vehicle is on the left, the azimuth φ<0. W is the standard lane width, dL the distance between the ego-vehicle and the left lane line, and dR is the distance between the ego-vehicle and the right lane line. Rr is the radius of the lane on which the target-vehicle travels, and Ro is the radius of the lane on which the ego-vehicle travels. D is the lateral distance between the ego-vehicle and the target-vehicle.Figure 9
Right turn curve lane relationship identification.It can be concluded from the cosine theorem in trigonometric functions that(22)Rr2=Ro2+r2−2Ro∗r∗cos90°−φ.Therefore, the lateral distance between the target-vehicle and the ego-vehicle is(23)D=Rr−Ro.①
Identification model when the target-vehicle and the ego-vehicle are in the same lane is(24)D≤dR+12B+12M,D<0,D≥dL+12B+12M,D>0,whereB is the width of the ego-vehicle and M is the lane marking line width.②
Identification model when the target-vehicle travels on the adjacent lane on the left side of the ego-vehicle is(25)dL+12B+12M<D<dL+12B+12M+W.③
Identification model when the target-vehicle travels on the adjacent lane on the right side of the ego-vehicle is(26)dR+12B+12M<D<dR+12B+12M+W.


## 4.6. Target Tracking Based on Improved Adaptive Extended Kalman Filter

As the vehicle travels on the road, the millimeter-wave radar detects targets in front of the vehicle, mainly including vehicles and road test isolation belts. These targets have no vertical motion or small moving speed in the vertical direction, so only the movement of the target-vehicle in theXY plane needs to be considered. Since the target-vehicle motion state is characterized by small mobility, a constant acceleration (CA) model is established based on the millimeter-wave radar motion coordinate system to describe the motion state of the front target-vehicle.When depicting the target motion, the target is regarded as a point in space regardless of its shape. The state of motion of the target is described in line with the laws of Newton’s kinematics. The model of the target motion state is as follows:(27)Xk+1=A∗Xk+B∗wk.The target motion state vectorX=x,y,x˙,y˙,x¨,y¨T here is designed to describe the motion state of the target-vehicle in the millimeter-wave radar Cartesian coordinate system, where x,y, x˙,y˙, and x¨,y¨ represent the position, velocity, and acceleration of the target-vehicle in the millimeter-wave radar Cartesian coordinate system, respectively.A is the state transition matrix, B is noise-driven matrix, and wk is the process noise at k:(28)A=10t0t220010t0t220010t000010t000010000001,B=t2200t22t00t0000.According to the principle of millimeter-wave radar, the millimeter-wave radar measures the relative radial distanceR and the relative radial velocity V between the target-vehicle and the ego-vehicle. The target tracking here is based on the vehicle's millimeter-wave radar coordinate system. The position of the target at k is assumed to be xk,yk, with velocityxk˙,yk˙. The observation Z consists of an acceleration and the relative radial distance R, as well as the relative radial velocity V between the target-vehicle and the ego-vehicle, where the relative radial distance R and the relative radial velocity V are measured from the millimeter-wave radar emission and the reflected wave, and the acceleration is calculated based on the ratio of the speed difference between the front and rear time and the sampling time. The observation equation is(29)Zk=Z1k=Rk=x2k+y2k+ν1k,Z2k=Vk=x˙2k+y˙2k+ν2k,Z3k=axk=xk˙−xk−1˙t+ν3k,Z4k=ayk=yk˙−yk−1˙t+ν4k,where ax is the longitudinal acceleration, ay is lateral acceleration, and vik is observation noise on k; t is the sampling time.Equations (25) and (27) are the state equation and the observation equation, respectively. As illustrated in the system model, the state equation is linear and the observation equation is nonlinear. Since the observation equation is nonlinear, the target tracking system based on the millimeter-wave radar is nonlinear.Since the statistical parameters of the observation noise of on-board millimeter-wave radar are often unknown and time-varying, the adaptive extended Kalman filter algorithm is adopted to calculate the statistical parameters of the noise. First, it estimates and updates the state of the system by iteration of state prediction and state correction and then calculates and corrects the noise statistical parameters online by noise statistic estimator.The extended Kalman filter (EKF) is an optimal estimation method for extracting the required signal from the sensor measurement signals based on a recursive algorithm and in line with the linear unbiased and minimum variance criteria. It is suitable for solving problems by computer and needs less computation and storage, so it has been widely applied in engineering [31, 32]. Its implementation steps are as follows.Step 1.
State prediction(30)x^kk−1=fx^k−1,uk−1,0,Pkk−1=Ak∗Pk−1∗ATk+Qk.Step 2.
State correction(31)Skk−1=Hk∗Pkk−1∗HTk+Rk,(32)Κk=Pkk−1∗HTk∗S−1kk−1,(33)x^k=x^k|k−1+Kk∗zk−z^kk−1,(34)PK=Pkk−1−Kk∗Hk∗Pkk−1.
For now, the adaptive extended Kalman filter algorithm with unbiased noise statistic estimator has been broadly applied. For example, the Sage extended Kalman filter (Sage-EKF) algorithm is used to estimate the observation noise and the system noise covariance matrix from residual sequence, so that the target motion state estimation is got from [33]. The unbiased noise statistic estimator includes the following steps:(35)εk=zk−z^kk−1,where z^kk−1=Hk∗x^kk−1.(36)dk=1−b1−bk+1,Rk+1=1−dkRk+dkεk∗εTk−Hk∗Pkk−1∗HTK,Qk+1=1−dkQk+dkKkεkεTkKTk+Pk−AkPk−1ATk,where εk is the innovation, dk is the forgetting factor at k, and b is the forgetting factor within the range of [0.95, 0.995]; b is 0.97 here.
However, matrix subtraction in the unbiased noise statistic estimator will lead to the loss of nonnegative definiteness of the covariance matrix of process noiseQ or the loss of positive definiteness of the covariance matrix of measurement noise R, thus weakening the robustness of the adaptive extended Kalman filter target tracking algorithm. Therefore, an improved adaptive extended Kalman filter algorithm, combining the unbiased noise statistic estimator and biased noise statistic estimator, is advanced in [34] to ensure the robustness of the adaptive extended Kalman filter algorithm. Its implementation steps are as follows:(37)Rk+1=Rk+1,ifRk+1is positive definite,1−dk∗Rk+dkεk∗εTk,otherwise,(38)Qk+1=Qk+1,ifQk+1is positive definite,1−dkQk+dkKkεkεTkKkT,otherwise.
Formulas (37) and (38) are biased noise estimators. They have the following two problems. On the one hand, the process noise covariance matrix Q discards some of the state estimation error covariance terms. On the other hand, the measurement noise covariance matrix R discards some of the measurement error covariance terms.
Since the discarded covariance term can correct the unbiased noise statistic estimator, the covariance matrix of process noiseQ and the covariance matrix of measurement noise R are inaccurate, thus reducing the accuracy of the adaptive extended Kalman filter. Based on [35], a new fault tolerant noise statistic estimator is proposed. The biased noise statistic estimator in [35] is as follows:(39)Rk+1=Rk+1,ifRk+1is positive definite,1−dk∗Rk+dkdiagzk−z^kk−12+HkPkHTk,otherwise,(40)Qk+1=Qk+1,ifQk+1is positive definite,1−dkQk+dkdiagx^k−x^kk−12−Pkk−1+Qk,otherwise.
The diag(■) in formulas (39) and (40) represents a diagonal matrix of the same dimension composed of diagonal elements of the operation matrix.
According to formula (34),(41)PK−Pkk−1=−Kk∗Hk∗Pkk−1.
By substituting formula (41) into (40), the following can be got:(42)Qk+1=Qk+1,ifQk+1is positive definite,1−dkQk+dkdiagx^k−x^kk−12+Kk∗Hk∗Pkk−1−Qk,otherwise,
Since the matrix subtraction may cause the process noise covarianceQ to lose nonnegative definiteness, the negative Q is rejected and the following can be obtained:(43)Qk+1=Qk+1,ifQk+1ispositive definite,1−dkQk+dkdiagx^k−x^kk−12+Kk∗Hk∗Pkk−1,otherwise.
According to formula (32),(44)Hk∗Pkk−1=Skk−1∗KTk.
By substituting formula (44) into (43), the following can be obtained:(45)Qk+1=Qk+1,ifQk+1is positive definite,1−dkQk+dkdiagx^k−x^kk−12+Kk∗Skk−1∗KTk,otherwise.
According to formulas (33) and (35),(46)x^k−x^kk−12=Kk∗εk∗εTk∗KTk.
By substituting formula (46) into (45), the following can be obtained:(47)Qk+1=Qk+1,ifQk+1is positive definite,1−dkQk+dkdiagKkεkεTkΚk+⋯+KkSk|k−1KTk,otherwise.
By substituting formula (37) into (35), the following can be obtained:(48)Rk+1=Rk+1,ifRk+1is positive  definite,1−dk∗Rk+dkdiagεkεTk+⋯+HkPkHTk,otherwise.
Formulas (47) and (48) present the new fault tolerant noise statistic estimator proposed herein. The new fault tolerant noise statistic estimator keeps the covariance terms from most unbiased noise statistic estimators and can improve the accuracy of the algorithm. As it can ensure that the covariance matrix of measurement noise R is always of positive definiteness and the covariance matrix of process noise Q is always of nonnegative definiteness, it delivers an improved robustness of the algorithm.
The implementation of the target tracking algorithm is based on the correct data association. If the data association is wrong, the motion state information obtained after the estimation will be of poor quality. Therefore, it is required to select a data association method with probability statistics to establish a correspondence between the target and the measurement.
A tracking gate is required for the data association algorithm to define the possible areas of measurement from the target by centering on the forecast position of the target [36]. The tracking gate has the following three functions: ① Determine the candidate measurement: when the measurement falls into the tracking gate area, it will be used to update the tracked target state. ② Establish a new target motion track: when the measurement does not fall into the tracking gate area, it will be taken as a new target or the existing interference clutter. ③ Confirm that the target is lost: when there is no measurement in the tracking gate for several continuing periods, the target will be considered lost.
The size and shape of the tracking gate play an important role. Its size depends on the probability of receiving the correct echo. It is generally rectangular or elliptical. Generally, the elliptic tracking gate is more widely used in engineering than the rectangular one. Hence, the elliptic tracking gate is adopted herein.
An NN data association method is proposed herein and is characterized by convenient implementation and a small amount of computation. Among the plurality of target measurement data within the tracking gate, the one closest to the predicted position is selected as the associated object of the target.
In the above adaptive extended Kalman filter algorithm, formula (35) gives the difference εk between the system observation value zk at the current moment k and the observation value z^kk−1 of the system state prediction at the previous moment, that is, the innovation.
The covariance matrix of the innovation is(49)Sk=EεkεTk=Ezk−z^kk−1zk−z^kk−1T.
The following can be got:(50)Sk=HkPkk−1HTk+Rk.
The statistical distance is defined as the weighted norm of the innovation vector:(51)d2k=εTkS−1kεk.
When the distance between the adjacent measurement vectors of the same targetd2k is less than a certain value λ, the target is considered to fall into the tracking gate, and the λ size may be calibrated according to the speed, distance, and the like of the object.(52)d2k≤λ.
As shown in Figure10, it is assumed that there is a track M and that the associated gate used for the forecast position is elliptical. There are observation points a, b, c, and d of the sensor near the elliptical tracking gate, where the point d is outside the associated gate, points a, b, and c are within the associated gate, and point c is the NN.Figure 10
NN data association.


## 5. Experiment and Discussion

### 5.1. Construction of the Experimental Platform

As shown in Figure11, to check the accuracy and validity of the proposed algorithm, Hongqi H7-PHEV vehicle is applied herein in the test.Figure 11
Experimental vehicle and environment. (a) Test road. (b) Driving environment. (c) Test platform equipment. (d) Experimental platform communication.
(a)(b)(c)(d)The test was conducted in three lanes with curves as shown in Figures11(a) and 11(b). Hongqi H7-PHEV test vehicle is in the middle lane. Three target-vehicles are in front of the Hongqi H7-PHEV test vehicle during the test and in the left lane, the same lane, and the right lane of Hongqi H7-PHEV test vehicle. The test starts at A and ends at B after passing the curve.The ego-vehicle is equipped with an RT3000 high-precision inertial navigator as shown in Figures11(c) and 11(d). RT3000 can output the lateral velocity, longitudinal velocity, and yaw rate of the ego-vehicle and deem the value of these variables as the ground truth.The ego-vehicle is equipped with MicroAutoBoxII and connected to the vehicle gateway port via the controller area network (CAN) to obtain the longitudinal velocity, yaw rate, and steering wheel angle information of the vehicle measured by ESC. MicroAutoBoxII is connected to RT3000 through CAN and gets the lateral velocity, longitudinal velocity, and yaw rate information of the ego-vehicle measured by RT3000. The ego-vehicle is equipped with a 77 GHz millimeter-wave radar, the target measurement data of which is open to the public, provided by a company, in the middle of the front bumper. The millimeter-wave radar is provided with two-way CAN. One CAN is connected to the vehicle gateway port. The other one is connected to MicroAutoBoxII through CAN. To simultaneously record the scene of the vehicle, the Logitech C930 webcam is mounted in the ego-vehicle cab rear view mirror.We use the Mobileye EyeQ4 camera to obtain information such as the lateral distance between the ego-vehicle and the lane line, the width of the lane, and the width of the lane marking. The Mobileye EyeQ4 is provided with two-way CAN. One CAN is connected to the vehicle gateway port to obtain information such as the yaw rate and speed of the vehicle during driving. The other one is connected to MicroAutoBoxII through CAN and outputs the information such as the lateral distance between the ego-vehicle and the lane line, the width of the lane, and the width of the lane marking.We use the lidar produced by Ibeo to detect the motion state information of the target-vehicle and take it as the ground truth, which is used to verify the performance of the target tracking algorithm of the millimeter-wave radar. In order to make the sensors smarter, Ibeo will provide point cloud processing algorithm software for the lidar. At present, the algorithm provided by Ibeo supports target recognition and tracking. The motion state information of the target-vehicle includes longitudinal distance and lateral distance. As shown in Figure11(c), the ego-vehicle is equipped with two lidars, which are lidar-1 and lidar-2. The Ibeo TrackingBox is responsible for data fusion of the two lidars.Table3 shows the specific technical specifications of the four-scan-level lidar provided by Ibeo.Table 3
Main technical parameters of the lidar.
ParameterValue (units)Ranging200 (m)Ranging resolution0.04 (m)Fov (H ∗ V)110∗ 3.2 (°)Vertical angle resolution0.8 (km/h)Update rate25 (Hz)


### 5.2. Analysis of Experiment Results

The on-board sensor output is used herein as the input of the Kalman filter to estimate the vehicle state in real time in an attempt to check the accuracy and effectiveness of the Kalman filter state observer based on 2-DOF vehicle dynamics model. The high-precision inertial navigator RT3000 is employed to measure the driving state of the vehicle. The estimated results are compared with the actual measurement data of the RT3000 at the end.According to the comparison results in Figures12–14, respectively, the results as illustrated show that the Kalman filter state observer based on the 2-DOF vehicle dynamics model has good consistency with the vehicle characteristics.Figure 12
Longitudinal velocity time history curve.Figure 13
Lateral velocity time history curve.Figure 14
Yaw rate time history curve.Experimental tests are carried out in three lanes with curves in this paper. The accuracy and robustness of the target tracking algorithm based on the IAEKF can be checked.In order to exclude the randomness of the experiments, we conducted 6 groups of experiments. Since the test results for the 6 groups of experiments are similar, one set of test data is selected for discussion and analysis. In this experiment, the three target-vehicles were driving in the left, middle, and right lanes, respectively. Since the lateral distance is a variable to identify the location relation of the lane where the target-vehicle was located, the lateral distance is visualized in Figure15.Figure 15
Target-vehicle lateral distance time history curve.The test results are shown in Figure15. In the figure, DL represents the left boundary of the center lane, DR represents the right boundary of the center lane, DLL represents the left boundary of the left lane, and DRR represents the right boundary of the right lane; and they are expressed as follows:(53)DL=dL+12B+12M,DR=dR+12B+12M,DLL=dL+12B+12M+W,DRR=dR+12B+12M+W.As can be seen from Figures11(b) and 15, the positional relationship of the three target vehicles can be accurately identified.During the experiment, in order to more clearly show the change between IAEKF estimation value and ground truth, the trajectory of the target-vehicle in the middle lane is visualized in Figure16.Figure 16
Target-vehicle in the center lane lateral distance time history curve.It can be concluded from Figure15 that the data fluctuation of the IAEKF algorithm during tracking the target is small. From equation (24) and Figure 16, it can be seen that the target-vehicle position relationship identification in the middle lane is correct.The mean square root error (MSRE) can be used to quantitatively analyze the filtering accuracy [37]. In order to quantitatively analyze the filtering accuracy of the IAEKF, Sag-EKF, and AEKF [38] algorithms, we counted the MSRE of the 6 groups of experiments, and the results are shown in Figures 17–19. Figures 17–19, respectively, show the MSRE of target-vehicle 1 in the left lane, target-vehicle 2 in the middle lane, and target-vehicle 3 in the right lane.Figure 17
MSRE curves of target-vehicle 1’s lateral distance for various filters.Figure 18
MSRE curves of target-vehicle 2’s lateral distance for various filters.Figure 19
MSRE curves of target-vehicle 3’s lateral distance for various filters.It can be concluded from Figures17–19 that, in the driving environment where the system noise is unknown and time-varying, the MSRE of the lateral distance of target tracking using Sag-EKF and AEKF algorithm is larger than that of IAEKF algorithm. Compared with Sag-EKF and AEKF algorithms, the maximum increase in filtering accuracy of lateral distance using IAEKF algorithm is 6.2% and 7.6%, respectively. Through the above analysis, it can be concluded that using IAEKF algorithm for tracking can effectively suppress the divergence of target tracking, thereby reducing the tracking error and improving the tracking accuracy.The target tracking algorithm is written in the environment of MatlabR2018a/Simulink in the host computer. The automatic code generation software provided by dSPACE company is used to download the code to MicroAutoBoxII1401 rapid prototyping controller through user datagram protocol (UDP) to run.As shown in Figure20, the time consumption of different algorithms is measured by the mean time of the algorithm running once in MicroAutoBoxII1401.Figure 20
Time mean of the algorithm running once.From Figure20, it can be concluded that the target tracking algorithm proposed herein takes 0.0078s at a time on average, slightly more than that of Sage-EKF and AEKF, where Sage-EKF and AEKF take 0.0064s and 0.0061s on average, respectively.In order to verify the accuracy of lane location relationship identification when the lane location relation of the target-vehicle changes, a lane-change test of the target-vehicle is designed. Figure21 shows a test scenario.Figure 21
Test scenario. (a) The start of the experiment. (b) The end of the experiment.
(a)(b)As shown in Figure21(a), target-vehicle 1 is located in the left lane and target-vehicle 2 is located in the middle lane at the start of the experiment. During the experiment, target-vehicle 1 was switched from the left lane to the right lane, and target-vehicle 2 remained in the middle lane at all times. During the experiment, the variation curve of the lateral distance difference between the target-vehicle and the ego-vehicle is shown in Figure 22.Figure 22
Identification result of target-vehicle position relationship.Before timet1, target-vehicle 1 is located outside the left boundary of the middle lane, so the lane where target-vehicle 1 is located is identified as the left lane. After time t4, target-vehicle 1 is located outside the right boundary of the middle lane, so the lane where target-vehicle 1 is located is identified as the right lane. Between t1and t4, target-vehicle 1 is identified as the middle lane. When target-vehicle 1 and target-vehicle 2 are traveling in the same lane, due to the occlusion of target-vehicle 1, target-vehicle 2 cannot be recognized by millimeter-wave radar between times t2 and t3.The above experiment’s analysis shows that the target-vehicles position relationship identification model proposed in this paper can accurately identify the lane location relationship of the target-vehicle.


## 5.1. Construction of the Experimental Platform

As shown in Figure11, to check the accuracy and validity of the proposed algorithm, Hongqi H7-PHEV vehicle is applied herein in the test.Figure 11
Experimental vehicle and environment. (a) Test road. (b) Driving environment. (c) Test platform equipment. (d) Experimental platform communication.
(a)(b)(c)(d)The test was conducted in three lanes with curves as shown in Figures11(a) and 11(b). Hongqi H7-PHEV test vehicle is in the middle lane. Three target-vehicles are in front of the Hongqi H7-PHEV test vehicle during the test and in the left lane, the same lane, and the right lane of Hongqi H7-PHEV test vehicle. The test starts at A and ends at B after passing the curve.The ego-vehicle is equipped with an RT3000 high-precision inertial navigator as shown in Figures11(c) and 11(d). RT3000 can output the lateral velocity, longitudinal velocity, and yaw rate of the ego-vehicle and deem the value of these variables as the ground truth.The ego-vehicle is equipped with MicroAutoBoxII and connected to the vehicle gateway port via the controller area network (CAN) to obtain the longitudinal velocity, yaw rate, and steering wheel angle information of the vehicle measured by ESC. MicroAutoBoxII is connected to RT3000 through CAN and gets the lateral velocity, longitudinal velocity, and yaw rate information of the ego-vehicle measured by RT3000. The ego-vehicle is equipped with a 77 GHz millimeter-wave radar, the target measurement data of which is open to the public, provided by a company, in the middle of the front bumper. The millimeter-wave radar is provided with two-way CAN. One CAN is connected to the vehicle gateway port. The other one is connected to MicroAutoBoxII through CAN. To simultaneously record the scene of the vehicle, the Logitech C930 webcam is mounted in the ego-vehicle cab rear view mirror.We use the Mobileye EyeQ4 camera to obtain information such as the lateral distance between the ego-vehicle and the lane line, the width of the lane, and the width of the lane marking. The Mobileye EyeQ4 is provided with two-way CAN. One CAN is connected to the vehicle gateway port to obtain information such as the yaw rate and speed of the vehicle during driving. The other one is connected to MicroAutoBoxII through CAN and outputs the information such as the lateral distance between the ego-vehicle and the lane line, the width of the lane, and the width of the lane marking.We use the lidar produced by Ibeo to detect the motion state information of the target-vehicle and take it as the ground truth, which is used to verify the performance of the target tracking algorithm of the millimeter-wave radar. In order to make the sensors smarter, Ibeo will provide point cloud processing algorithm software for the lidar. At present, the algorithm provided by Ibeo supports target recognition and tracking. The motion state information of the target-vehicle includes longitudinal distance and lateral distance. As shown in Figure11(c), the ego-vehicle is equipped with two lidars, which are lidar-1 and lidar-2. The Ibeo TrackingBox is responsible for data fusion of the two lidars.Table3 shows the specific technical specifications of the four-scan-level lidar provided by Ibeo.Table 3
Main technical parameters of the lidar.
ParameterValue (units)Ranging200 (m)Ranging resolution0.04 (m)Fov (H ∗ V)110∗ 3.2 (°)Vertical angle resolution0.8 (km/h)Update rate25 (Hz)


## 5.2. Analysis of Experiment Results

The on-board sensor output is used herein as the input of the Kalman filter to estimate the vehicle state in real time in an attempt to check the accuracy and effectiveness of the Kalman filter state observer based on 2-DOF vehicle dynamics model. The high-precision inertial navigator RT3000 is employed to measure the driving state of the vehicle. The estimated results are compared with the actual measurement data of the RT3000 at the end.According to the comparison results in Figures12–14, respectively, the results as illustrated show that the Kalman filter state observer based on the 2-DOF vehicle dynamics model has good consistency with the vehicle characteristics.Figure 12
Longitudinal velocity time history curve.Figure 13
Lateral velocity time history curve.Figure 14
Yaw rate time history curve.Experimental tests are carried out in three lanes with curves in this paper. The accuracy and robustness of the target tracking algorithm based on the IAEKF can be checked.In order to exclude the randomness of the experiments, we conducted 6 groups of experiments. Since the test results for the 6 groups of experiments are similar, one set of test data is selected for discussion and analysis. In this experiment, the three target-vehicles were driving in the left, middle, and right lanes, respectively. Since the lateral distance is a variable to identify the location relation of the lane where the target-vehicle was located, the lateral distance is visualized in Figure15.Figure 15
Target-vehicle lateral distance time history curve.The test results are shown in Figure15. In the figure, DL represents the left boundary of the center lane, DR represents the right boundary of the center lane, DLL represents the left boundary of the left lane, and DRR represents the right boundary of the right lane; and they are expressed as follows:(53)DL=dL+12B+12M,DR=dR+12B+12M,DLL=dL+12B+12M+W,DRR=dR+12B+12M+W.As can be seen from Figures11(b) and 15, the positional relationship of the three target vehicles can be accurately identified.During the experiment, in order to more clearly show the change between IAEKF estimation value and ground truth, the trajectory of the target-vehicle in the middle lane is visualized in Figure16.Figure 16
Target-vehicle in the center lane lateral distance time history curve.It can be concluded from Figure15 that the data fluctuation of the IAEKF algorithm during tracking the target is small. From equation (24) and Figure 16, it can be seen that the target-vehicle position relationship identification in the middle lane is correct.The mean square root error (MSRE) can be used to quantitatively analyze the filtering accuracy [37]. In order to quantitatively analyze the filtering accuracy of the IAEKF, Sag-EKF, and AEKF [38] algorithms, we counted the MSRE of the 6 groups of experiments, and the results are shown in Figures 17–19. Figures 17–19, respectively, show the MSRE of target-vehicle 1 in the left lane, target-vehicle 2 in the middle lane, and target-vehicle 3 in the right lane.Figure 17
MSRE curves of target-vehicle 1’s lateral distance for various filters.Figure 18
MSRE curves of target-vehicle 2’s lateral distance for various filters.Figure 19
MSRE curves of target-vehicle 3’s lateral distance for various filters.It can be concluded from Figures17–19 that, in the driving environment where the system noise is unknown and time-varying, the MSRE of the lateral distance of target tracking using Sag-EKF and AEKF algorithm is larger than that of IAEKF algorithm. Compared with Sag-EKF and AEKF algorithms, the maximum increase in filtering accuracy of lateral distance using IAEKF algorithm is 6.2% and 7.6%, respectively. Through the above analysis, it can be concluded that using IAEKF algorithm for tracking can effectively suppress the divergence of target tracking, thereby reducing the tracking error and improving the tracking accuracy.The target tracking algorithm is written in the environment of MatlabR2018a/Simulink in the host computer. The automatic code generation software provided by dSPACE company is used to download the code to MicroAutoBoxII1401 rapid prototyping controller through user datagram protocol (UDP) to run.As shown in Figure20, the time consumption of different algorithms is measured by the mean time of the algorithm running once in MicroAutoBoxII1401.Figure 20
Time mean of the algorithm running once.From Figure20, it can be concluded that the target tracking algorithm proposed herein takes 0.0078s at a time on average, slightly more than that of Sage-EKF and AEKF, where Sage-EKF and AEKF take 0.0064s and 0.0061s on average, respectively.In order to verify the accuracy of lane location relationship identification when the lane location relation of the target-vehicle changes, a lane-change test of the target-vehicle is designed. Figure21 shows a test scenario.Figure 21
Test scenario. (a) The start of the experiment. (b) The end of the experiment.
(a)(b)As shown in Figure21(a), target-vehicle 1 is located in the left lane and target-vehicle 2 is located in the middle lane at the start of the experiment. During the experiment, target-vehicle 1 was switched from the left lane to the right lane, and target-vehicle 2 remained in the middle lane at all times. During the experiment, the variation curve of the lateral distance difference between the target-vehicle and the ego-vehicle is shown in Figure 22.Figure 22
Identification result of target-vehicle position relationship.Before timet1, target-vehicle 1 is located outside the left boundary of the middle lane, so the lane where target-vehicle 1 is located is identified as the left lane. After time t4, target-vehicle 1 is located outside the right boundary of the middle lane, so the lane where target-vehicle 1 is located is identified as the right lane. Between t1and t4, target-vehicle 1 is identified as the middle lane. When target-vehicle 1 and target-vehicle 2 are traveling in the same lane, due to the occlusion of target-vehicle 1, target-vehicle 2 cannot be recognized by millimeter-wave radar between times t2 and t3.The above experiment’s analysis shows that the target-vehicles position relationship identification model proposed in this paper can accurately identify the lane location relationship of the target-vehicle.


## 6. Conclusions

In this paper, the data rationality judgment method is employed to eliminate the target measurement data outside the radar detection range. By analyzing the effectiveness of the target, the false target and clutter can be removed. The Kalman filter algorithm based on 2-DOF vehicle dynamics model is utilized to filter the longitudinal velocity, lateral velocity, and yaw rate of the ego-vehicle. The filtered longitudinal velocity, lateral velocity, and yaw rate are used for motion compensation of the target-vehicle distance and velocity detected by the millimeter-wave radar. Targets clustering is performed through Manhattan distance, so that multiple target measurement data are clustered into one. It is possible to accurately identify the positional relationship of the lane where the target-vehicle locates by establishing an accurate lane relationship identification model. The robustness and accuracy of the target tracking algorithm are improved by the IAEKF algorithm and the NN data association algorithm. Finally, the vehicle test verifies that the algorithm proposed herein can accurately identify the lane position relationship of target-vehicle. Compared with Sag-EKF and AEKF algorithms, the maximum increase in filtering accuracy of lateral distance using IAEKF algorithm is 6.2% and 7.6%, respectively. The target-vehicles position relationship identification model proposed in this paper can accurately identify the lane location relationship of the target-vehicle.



---

*Source: 3749759-2020-06-27.xml*