Trajectory Smoothing Algorithm Based on Kalman Filter

Due to the error of positioning system and signal interference, the real tracking and collected trajectory data usually have multipath effect, and multipath noise obeying bimodal distribution will appear in the trajectory. To verify the different performance of the Kalman filter on Gaussian and non-Gaussian noise, a trajectory smoothing algorithm based on Kalman filter is proposed and experimented on simulated and real trajectory sets. After adding Gaussian noise and non-Gaussian multipath noise to the velocity and position of the trajectory, respectively. The RMSE max for velocity and position in the 1D trajectory set is 0.21 and 1.11. In the 2D trajectory set, it is 0.97 and 1.51, respectively. And the RMSE max of latitude and longitude in the real trajectory set is 3.272 × 10 ⁻⁵ and 5.589 × 10 ⁻⁵ . Experimental results show that the algorithm can smooth Gaussian noise well, but does not achieve good performance in non-Gaussian noise, although it can reduce the effect of multipath noise on the trajectory position.