For the research of localization and navigation

it is necessary to restore three-dimensional motion trajectory of objects in indoor scenes at different levels

a new method based on improved Mahony complementary filtering algorithm is proposed to solve restoring three-dimensional motion trajectory problem. The experiment uses built-in motion sensor of smartphones and the MPU6050 nine-axis motion sensor. In this experiment

only acceleration and angular velocity data of the sensor are used. The steps are as follows: first

those data are preprocessed by Kalman filter. Then update the rotation matrix by using quaternion. Finally

the displacement is obtained by integral and based on that the trajectory is obtained by adding proper compensation. In order to verify the reliability of the improved algorithm

trajectory curve of this algorithm is compared with the traditional complementary filtering algorithm and the classical fourth-order Runge-Kutta algorithm. The result shows that the improved Mahony complementary filtering algorithm can obtain the trajectory more accurately and robustly

which basically meets need of restoring the trajectory of three-dimensional motion.