Abstract: To achieve 3D pedestrian trajectory tracking, a navigation block tied to the walkers' shoes is used, which combines inertial measurement unit (IMU) and electronic compass to track pedestrian trajectory. Regarding the characteristics of pedestrian movement cycle, an improved Attitude and Heading Reference System (AHRS) algorithm with quaternion is considered to accurately estimate the walking attitude, and especially the heading. After analyzing kinds of reasons for velocity errors, a new zero velocity updata algorithm is developed. Height compensation is realized through judging pedestrian walking state according to the vertical walking speed, and thus 3D pedestrian trajectory is accurately tracked. Some experiments are conducted to demonstrate the effectiveness and feasibility of the proposed method, including straight line round-trip walking, rectangle line walking, and walking up-and-down stairs. In the experiments, the deviation of the 2D trajectory is about 0.5 m and the deviation of the 3D trajectory is about 1 m.