Chapter 5 Experiment Result and Comparison
5.3 Objection Localization and Tracking
In this section three methods of deriving target’s coordinate are tested, including Least Square, Newton Ralphson’s method, and EKF. At first all the 5 channels are used among the three methods, then the outlier rejection issue would then be discussed in
39
Table 5.3-2 : Marker pen, mean of target coordinate (x,y) among different methods (unit: cm)
Target Localization
Method
Least Square Newton’s
method EKF
Target
Coordinate
axis
(14, 37) x
13.44 13.48 13.49y
36.56 38.51 38.49(18, 37) x
17.74 17.80 17.79y
35.84 38.58 38.55(14, 41) x
14.09 14.07 14.10y
41.72 41.56 41.28(18, 41) x
18.24 18.23 18.22y
41.05 41.60 41.3540
Figure 5.3-1 shows the XY-plot of the three methods with coordinate of marker pen
at (18, 41).
(a) (b)
(c)
Figure 5.3-1 : Object localization comparison a) Least Square
b) Newton’s Method c) EKF
Note that the ground truth is measured manually.
R1
R2 R3
R1
R2 R3
41
Case2:
Table 5.3-3 and Table 5.3-4 show the standard deviation and mean of each target coordinate, we can see that the standard deviation of target coordinate is the smallest using EKF.
Table 5.3-3 : Human’s index finger, standard deviation comparison of target coordinate (x, y) (unit: cm) Target
Localization Method
Least Square Newton’s
method EKF
Target
Coordinate
axis
(14, 37) x 1.191 1.336 0.400
y 3.514 0.498 0.143
(18, 37) x 0.995 0.968 0.216
y 3.521 0.430 0.222
(14, 41) x 1.734 1.036 0.574
y 5.203 0.448 0.292
(18, 41) x 1.449 1.178 0.718
y 4.146 0.530 0.246
42
Table 5.3-4 : Human’s index finger, mean of target coordinate (x, y) among different methods (unit: cm)
Target Localization
Method
Least Square Newton’s
method EKF
Target
Coordinate
axis
(14, 37) x 13.72 13.64 13.72
y 35.69 37.97 37.89
(18, 37) x 17.70 17.70 17.70
y 36.65 37.94 37.75
(14, 41) x 13.42 13.41 13.56
y 42.11 41.60 41.23
(18, 41) x 17.87 17.83 17.86
y 40.72 41.39 41.00
43
Figure 5.3-2 shows the XY-plot of the three methods with coordinate of human’s finger at (18, 41) .
(a) (b)
(c)
Figure 5.3-2 : Object localization comparison a) Least Square
b) Newton’s Method c) EKF
Note that the ground truth is measured manually.
R1
R2 R3
R1
R2 R3
44
Case3: target as human’s index finger
(*Note that blue dots is the estimated coordinate, red line is the reference target
trajectory. Finger stopped at the starting point and end point for a while before and after moving.) (1) Target moves from (8, 40) to (20, 40), velocity of x direction is about 1cm/sec.
Figure 5.3-3 : Object tracking comparison a) Least Square
b) Newton’s Method c) EKF
(a) (b)
(c)
R1
R2 R3
R1
R2 R3
45
Figure 5.3-4 : The EKF states change of x and y direction from Figure 5.3-3 (c)
46
(2) Target moves from (8, 36) to (20, 48), velocity of target is about 2 cm/sec.
Figure 5.3-5 : Object tracking comparison a) Least Square
b) Newton’s Method c) EKF
(a) (b)
(c)
R1
R2 R3
R1
R2 R3
47
Figure 5.3-6 : The EKF states change of x and y direction from Figure 5.3-5 (c)
48
(3) Target moves from (20, 36) to (20, 48), velocity of y direction is about 1cm/sec.
Figure 5.3-7 : Object tracking comparison a) Least Square
b) Newton’s Method c) EKF
(a) (b)
(c)
R1
R2 R3
R1
R2 R3
49
Figure 5.3-8 : The EKF states change of x and y direction from Figure 5.3-7 (c)
50
5.3.2 Comparison and Discussion
We first observe the least square problem from (4.1-3), the system matrix A that contains the range estimate would increase uncertainty of estimation. Newton’s method is better than least square method since the round trip model doesn’t contain any range estimation. However the Newton’s method only estimates the coordinate based on the current TOF data so that the estimation result would be directly affected by the measurement noise. Although the performance of Newton’s method and EKF are similar when target is stationary, the EKF can still provide more stable and smoother target localization and tracking since it estimates the target coordinate based on the previous state and inherently considers the interference of measurement.
Note that from Figure 5.3-4 and Figure 5.3-6 , the estimated velocity from EKF is close to the target moving velocity, but comparing the (x, y) estimation in Figure 5.3-5(c), tracking on x direction is delayed more than on y direction. It is because that the geometry of ultrasonic platform structure (Appendix and [13]) makes the variance of x direction bigger than y direction (we can also observe this phenomenon in Table 5.3-1 and Table 5.3-3 ). To solve this problem we simply set the process noise covariance of x direction smaller than that of y direction, which means that it would reduce the variance of the estimated x coordinate but the tradeoff is that the delay would increase when tracking on x direction. Finally from Table 5.3-2 and Table 5.3-4, it is observed that the estimated means of target coordinate are similar among three methods.
We then consider the outlier issue in case3, when target moves from (20, 36) to (20, 48), we observe that sometimes echo would be distorted seriously hence several outliers appear in measurement (Figure 5.3-9). The outliers cause a great effect on the tracking performance of EKF. In this section we use the outlier detection method mentioned in section 4.3 to improve the performance of tracking on target:
51
Figure 5.3-9 : Measurement data of Case3 (3)
52
Case3: target as human’s index finger
(3) Target moves from (20, 36) to (20, 48), velocity of y direction is about 1cm/sec
Figure 5.3-10 : Object tracking comparison a) single EKF
b) EKF with outlier rejection
(a) (b)
53
Figure 5.3-11 : The EKF states change of x and y direction from Figure 5.3-10 (b)
54
(1) Target moves from (8, 40) to (20, 40), velocity of x direction is about 1cm/sec
Figure 5.3-12 : Object tracking comparison a) single EKF
b) EKF with outlier rejection
(a) (b)
55
Figure 5.3-13 : The EKF states change of x and y direction from Figure 5.3-12 (b)
56
(2) target moves from (8,36) to (20, 48), velocity of target is about 2 cm/sec.
Figure 5.3-14 : Object tracking comparison a) single EKF
b) EKF with outlier rejection
(a) (b)
57
In case3 (3),we can see that when the outliers are detected and rejected, the variance of x coordinate in Figure 5.3-11 is relative smaller than Figure 5.3-8, and the estimated target trajectory in Figure 5.3-10 (b) can follow up the desired path smoother and more consistent than in Figure 5.3-10 (a). Note that since there is no obvious outlier in measurement in case 3 (1) and (2), the results of Figure 5.3-12 (a)(b) and are similar, so do the results in Figure 5.3-14 (a)(b).
Figure 5.3-15 : The EKF states change of x and y direction from Figure 5.3-14 (b)
58
Finally we observe another trajectory using the EKF with outlier detection.
Case3: target as human’s index finger
(4) target moves along circle with center at (16, 42), both the starting point and the end point are at (16, 48).
From Figure 5.3-16 we see that the estimated target coordinate is around the reference trajectory. However there is delay of x direction near the end point. This is caused by the outliers that continuously appear when target is moving, and since the Kalman gain would be small during presence of outliers, it would make the estimated coordinate close to prediction trajectory.
Figure 5.3-16 : Target moves along circle
59