@TobiasM Thank you for your advice. I tried this in Gazebo Simulation. I find that the drone tracks good in X-axis BUT seems that it doesnt work in Y-axis(I abandoned Z, Z is set to 2 ).
Below are part of the position log:
time
goal.x goal.y goal.z
real.x real.y real.z
17.350000
14.421766 0.179454 2.000000
14.201502 -0.499506 2.034843
17.400000
14.475429 0.247687 2.000000
14.244723 -0.445643 2.034098
17.450000
14.528336 0.315501 2.000000
14.313771 -0.359543 2.032773
17.500000
14.580449 0.382812 2.000000
14.347980 -0.314730 2.032230
17.550000
14.631733 0.449536 2.000000
14.416365 -0.225706 2.031084
17.600000
14.682151 0.515590 2.000000
14.459023 -0.170487 2.030164
17.650000
14.731667 0.580893 2.000000
14.526984 -0.082826 2.028497