Abstract: To improve the data detection accuracy of low-cost sensors in the original navigation system and the control effect of ship trajectory tracking, the data fusion algorithm was introduced to propose a new trajectory tracking algorithm. The multiple coordinate transformations of ship hull were conducted, and two Kalman filters were used to fuse more accurate ship heading angle, coordinates and velocity. The control algorithm based on eliminating trajectory velocity deviation and heading angle deviation was proposed. The traditional PID controller was designed as cascade control system with PI controller and differential controller. The differential variables in the inner loop were precisely detected and transmitted by the existing accelerometers and gyroscopes to eliminate the adjusting differential parameters for achieving good control results of systems with low detection accuracy. The each required output of the two vessels on the ship was calculated to improve the effect of trajectory tracking. The multiple cruise tests of unmanned working ship were conducted on the experimental platform with real-time monitoring the ship status data and operating trajectory through upper computer, and the overall optimized trajectory tracking effect was analyzed. The results show that the heading angle, positioning and velocity fused by the Kalman filter have smaller data variance and are closer to ideal values. For the control algorithm, the overshoot of the system does not exceed 3%, and the response speed is fast enough with the steady-state error of about zero. After optimization, the success rate of the straight section for trajectory tracking is increased from 80% to 95%, and the success rate of the turning section is increased from 60% to 90%. The maximum average yaw distance of the straight section after optimization is decreased from 0.83 m to 0.12 m, and the maximum average yaw distance of the turning section is decreased from 1.25 m to 0.22 m. After turning, the average adjustment distance of 0.95 meters is required to enter the straight state, and the tracking effects of straight and turning sections are significantly improved.