Modified extended object tracker for 2D lidar data using random matrix model

The random matrix (RM) model is a typical extended object-modeling method that has been widely used in extended object tracking. However, existing RM-based filters usually assume that the measurements follow a Gaussian distribution, which may lead to a decrease in accuracy when the filter is applied to the lidar system. In this paper, a new observation model used to modify an RM smoother by considering the characteristics of 2D LiDAR data is proposed. Simulation results show that the proposed method achieves a better performance than the original RM tracker in a 2D lidar system.

• A novel method is proposed to estimate the kinematic vector and extended matrix of the object.
• The proposed method is applied to the Bayesian filtering method described in 16 , which is a robust and effective RM tracker. • The performances of the proposed and original methods with LiDAR data are compared.
The remainder of this paper is organized as follows. Section "Problem statements" introduces tracking problems. In Section "Modified Rm model for 2D lidar", the RM model is improved based on the detection characteristics of 2D lidar data. Section "Implementation" presents the modified RM tracker for a 2D lidar system. Section "Results" presents the simulation results. Finally, Section "Conclusion" provides some concluding remarks.

Problem statements
Let ξ k denote the extended state at time k: The RM model is usually defined as ξ k = {x k , X k } , where x k is the kinematical vector, X k is an d × d matrix used to describe the extent of the object, and d is the dimension of the tracking scenario. Using a Bayesian filtering framework, the state prediction is where p x k , X k |x k−1 , X k−1 is the density of the transition. The state update is written as The RM model assumes that x k follows a Gaussian distribution and X k follows an inverse Wishart distribution. Therefore, according to 18 , the object state density based on the factorized model is where N(·) denotes a Gaussian distribution and IW(·) denotes an inverse Wishart distribution.
In the original random matrix model shown in Fig. 1(a), for the measurement set Z k = z (i) k , it is typically assumed that each z (i) k is generated independently from the Gaussian distribution. Therefore, x k and X k are usually estimated using the mean and covariance of the measurement set, Z k , respectively. However, in the 2D lidar system, as shown in Fig. 1(b), the measurements are distributed along the contour of the object, and only one side can be detected. The mean and covariance of Z k cannot accurately describe the real position or extent of an object. Therefore, as a problem with the RM model, it cannot estimate the real state of an object when the data are from a 2D lidar system.
In this study, the detection process of the 2D lidar system was analyzed and the RM model was modified by considering the characteristics of the lidar data.
L denote the distance and radian of the i th ranging per revolution, respectively. According to the detection principle of lidar when applied in the real world (as shown in Fig. 2), the following assumptions were made:

Assumption 1
The lidar applies a detection n r times per revolution, and the i th observation radian r (i) L follows a Gaussian distribution N r (i) L |i · 2π n r , σ 2 r , where σ 2 r is the given variance.

Assumption 2
The noise range follows a zero-mean Gaussian distribution with σ 2 d as the variance.
Assumption 3 According to interference factors, such as weather or strong light, the object has a detection probability of p D 1 .

Assumption 4
If an object can be detected according to the difference in its surface material, the detection probability of each range is p D 2 .
Based on the above assumptions, the measurement model can be described as follows: denote the measurement set per revolution. According to Assumption 3, there is a probability of 1 − p D 1 that Z is an empty set.
• When Z is not empty, according to Assumption 4, there is a probability of 1 − p D 2 that z (i) is an empty set. where c(·) denotes the detectable contour function of the object. For example, c ẑ, µ L , r (i) L is the ranging result of an object with a center coordinate ẑ . The coordinates of the lidar is µ L , and the detection radian is r Proposed method. Let the lidar coordinate system be the center of the coordinate system. Given a meas- in Cartesian coordinates at time k (for convenience, polar coordinates are not used), according to Eq. (4), state estimation errors will be caused when using the mean of Z k . Assuming that the extent of an object can be approximated as an ellipse, its center ẑ k can be approximated as follows: where µ 1 and µ 2 are two points on straight line l 3 passing through the center of the ellipse. According to Assumption 1, two measurements with minimum and maximum radians can be approximated as two tangent points on the tangent line of an ellipse passing through the lidar coordinates. Therefore, the corresponding ellipse tangent   Fig. 2) can be obtained from the coordinates of the two measurement points and the lidar coordinates. As shown in the left half of Fig. 2, µ 1 and µ 2 can be approximated from the perpendicular tangent of the ellipses l 1 and l 2 . This is because according to the mathematical properties of an ellipse, l 1 and l 2 are the bisectors of the angle formed by the tangent point and focuses of the ellipse. Therefore, the distances from the center to µ 1 and µ 2 are similar. Note that if the target is small or far from the lidar, the error of (5) will be small. This occurs in this case because the included angle of the two tangent lines is extremely small, and the two straight lines can be regarded as parallel; thus, Eq. (5) is sufficiently accurate. According to the detection mode of the lidar, z (1) k and z (n k ) k can be approximated as two tangent points. Suppose that z The line l 3 passing through the center of the ellipse can be estimated based on Z k . As shown in the right half of Fig. 2, according to Assumption 1, the polar coordinate system can be divided into sectors based on the radian 2π n r , and the number of measurements in each sector may be zero, 1, or higher. By matching the measurements in the corresponding sector, the set Z k = {z i } n i=1 of the midpoints can be obtained. Note that if there are multiple measurements in a sector, the mean value of these measurements is used for the calculation. If there are no measurements in a sector, then the measurements in its paired sectors do not participate in the calculation of Z k . Supposing that z i = x i , y i , l 3 can be fitted using the least square method 19,20 , i.e., The equations for l 1 , l 2 , and l 3 have been solved; thus, µ 1 and µ 2 can be obtained by Note that if the number n is too small, the equation of l 3 may be inaccurate; thus, a threshold of ϑ must be given. When n > ϑ , ẑ k will be obtained using the proposed method, whereas when n ≤ ϑ , ẑ k will be solved based on the weighted mean of Z k , that is, Eq. (5) should be rewritten as where ω (i) is the weight of z (i) k . Equation (10) uses the weighted form because the inclination of the object contour affects the measurement density (as shown in Fig. 2). Therefore, ω (i) can be calculated as are the distance and radians of the i th and j th measurements, respectively; and τ denotes normalization. In fact, the numerator of Eq. (11a) represents the length of the red line in Fig. 2, whereas the denominator is the length of the blue line. By weighting Eq. (11), the influence of the object contour inclination on the statistical accuracy can be reduced. Similarly, the covariance matrix Ŷ of the measurement set Z k can be obtained as

Implementation
In this section, the proposed method is used to modify the advanced RM tracker presented in 16 . The tracker comprises three steps: prediction, updating, and smoothing. Let function M(x) denote the change mode of the object extent using factorized Gaussian inverse Wishart densities 18 . If M(x) is a given d × d invertible matrix A , the prediction steps are as follows.
where m k+1|k and P k+1|k are the motion state and corresponding covariance, respectively; V k+1|k and v k+1|k are the Wishart scale matrix and degree of freedom, respectively; f k (·) and F k are the motion model and corresponding state transition matrix, respectively; and Q k denotes the process noise covariance. If M(x) is not a given matrix, formulas (13c) and (13d) become: where Tr{·} is the trace of a matrix, and E k−1|k−1 [·] is is the expectation.
The state updating steps are as follows: Here, κ is a given scale parameter, and R is the object observation noise. In addition, ẑ and Ẑ can be calculated using the proposed method, that is, Eqs. (10) and (12).
Note that this study did not contribute to the smoothing steps. Therefore, the smoothing steps are the same as those for the original smoother and can be found in Section V of 16 .

Results
This section presents the simulation results of the proposed method and the original approach presented in 16 . There are three scenarios: an ellipse object scenario, a rectangular object scenario, and 2D lidar data scenario. Note that the first two scenarios are based on simulation, whereas the last one uses a real dataset. The lidar parameters are as follows: 3irobotix-3i-T1 2D Lidar, 20-m detection radius, 1800 detections per revolution, and a scan interval of T = 0.1 s. The motion model is a coordinated turn model, that is, the object motion state is where the lidar scan interval is T = 1 s. In addition, I and O denote the identity matrix and zero matrix, respectively. Measurements were generated using the model proposed in Section "Measurement model of 2D lidar". The detection probability of the object was p D 1 = 0.9 , and the detection probability of each range was p D 2 = 0.9 . The Lidar conducts a detection 7200 times per revolution, and the observation noise is σ d = 0.1 2 , σ r = 0.001 2 .
The performances of different methods are judged based on the Gaussian Wassterstein distance (GWD) metric [21][22] , which considers both the object location and extension errors, and is defined as follows: (15a) m k|k = m k|k−1 + Kε, where k|l is the GWD value at time k. The first half describes the square of the location error, and the second half describes the square of the extension error. The GWD metric is used to indicate the similarity of the two distributions, that is, the smaller the value k|l , the more similar the two distributions are.
Ellipse object scenario. In this section, the real shape of the object is an ellipse. Figure 3 shows the filtering and smoothing results of 200 Monte Carlo runs. The GWD errors of the proposed method are lower than those of the original method, which shows that the proposed method improves the tracking accuracy. Figure 4 shows the tracking results for a single run. It can be seen intuitively that, compared with the original method, the object motion and extent states estimated by the proposed method are closer to the real situation, which is also the reason for the difference in the errors in Fig. 3. Figure 5 shows the average location and extension errors of 200 Monte Carlo runs. It can be seen that the difference between the location errors of the proposed method and the original filter is greater than the extension errors. This shows that the method proposed in Section "Modified Rm model for 2D lidar" can effectively estimate the location of the target center, which is the main reason for the improvement in the tracking performance.  Although it can be observed that the proposed algorithm has a higher tracking accuracy, the time cost is approximately twice that of the original algorithm. Therefore, the proposed method is more suitable for 2D lidar systems that are insensitive to such cost.

Rectangular object scenario.
In this section, the target is rectangular. The shape of the vehicle is often rectangular in the real world; thus, the rectangular object can be used to test the performance of the proposed method for the tracking of non-elliptical objects. Figure 7 shows the filtering and smoothing results of 200 Monte Carlo runs. The blue and red ellipses represent the results obtained using the original and proposed methods, respectively. The green rectangle indicates the vehicle location. The black square represents the location of the lidar. The proposed method still achieves a better performance. Figure 8 shows the results of a single run. It can be seen that although the shape of the object is rectangular, the proposed method can still use an ellipse to approximate its shape. Therefore, the proposed method is robust and can be applied to real scenarios such as vehicle tracking. Figure 9 shows the average location and extension errors of the rectangular object. It can be observed that the proposed method is still effective in reducing the location errors compared with the original filter when tracking a rectangular object. At the same time, the problem of a high time cost, shown in Fig. 10, still exists; thus, users should comprehensively consider the balance between accuracy and time cost.
Real 2D lidar data scenario. In this section, real-world 2D Lidar data used to test the effectiveness of the proposed method are described. The object in the scenario was a car, and 60 data scans were obtained using 2D lidar. The vehicle turns left on the open ground (as shown in Fig. 11). Note that the location data of the vehicle comes from manual markings.    www.nature.com/scientificreports/ Figure 11 shows the tracking results of five scans. The green rectangle indicates the vehicle position. It can be observed that the tracking performance of the proposed method is better than that of the original approach. Figure 12 shows the filtering and smoothing results. It can be seen that the errors of both the proposed and original methods are large and similar at 1.5 to 3 s. According to Fig. 11, the car is turning during this period, and the lidar measurement is significantly affected by the environment and the inclination angle of the vehicle surface. According to Eq. (10), when the number of midpoints is relatively small, that is, n < ϑ , the proposed method becomes.
k . It is the weighted average of the measurement taking into account the influence of the object contour inclination and similar to the original RM method (see the Eq. (39) in 8 ), which is why the accuracy of the proposed method is similar to that of the original method in 1.5 to 3 s. Therefore, the tracking accuracies of the proposed and original methods are significantly reduced. However, the accuracy of the proposed method is higher than that of the original method for other time periods, which shows that the proposed method is effective for use with real 2D LiDAR data. Figure 13 shows the location and extension errors. The location errors of both the proposed and original methods increase significantly within 1.5 to 3 s, whereas the expansion error fluctuates less during this period.   Figure 14 shows the time cost. Similar to the simulation scenarios, the time cost of the proposed method for processing real 2D Lidar data is higher than that of the original method.

Conclusion
In this paper, a modified RM model was proposed for tracking an extended object using a 2D lidar system. Compared to the original model, the proposed method can better estimate the location and extended state of an object using the physical characteristics of the lidar system. The simulation results show lower GWD errors for the proposed method than for the original approach. Although some RM-based tracking methods have also discussed the tracking problem in a lidar system, the tracking results should be calculated according to the characteristics of specific objects. For example, in 17 , information such as the front direction and steering angle of the front wheels was used; thus, the method is only suitable for the tracking of rectangular vehicles. The proposed method provides another interesting idea, i.e., a way to make full use of the measurement characteristics of a lidar system to estimate the real state of an object. Therefore, the proposed method is not limited to vehicle tracking. However, it should be noted that the method is only applicable to lidar systems with a high measurement accuracy. If the amount of noise is excessive, the results calculated by Eqs. (8) and (9) may have large errors, which is also a problem with the proposed approach.
In a future study, we plan to apply the proposed method to multiple extended object-tracking filters, such as a PMBM filter 12 .

Data availability
The data generated during this study are included in this article. Time cost (s) 10 -4 Original Proposed Figure 14. Time cost of real 2D lidar data scenario.