Kalman Filter-based Path Following Control Design for Unmanned Surface Vehicles

Abstract

Accurate state estimation is essential for reliable path-following control of unmanned surface vehicles (USVs), especially when low-cost sensors and nonlinear maneuvers are involved. In small USVs, heading and position measurements obtained from magnetic compasses and single GNSS modules are often affected by noise and disturbances, which degrade estimation accuracy in closed-loop control systems. To address this problem, this study establishes a unified closed-loop framework integrating line-of-sight (LOS) guidance, heading control, and Bayesian state estimation. First, a high-fidelity USV dynamic model is developed to simulate realistic nonlinear maneuvering behaviors. Second, multiple filtering algorithms, including KF, EKF, UKF, CKF3, and CKF5, are systematically implemented under compass-based and GNSS-based measurement schemes. Finally, Monte Carlo simulations are conducted in different maneuvering scenarios-straight-line motion, waypoint switching, and constant rudder turning—to evaluate estimation accuracy and computational efficiency. The results show that KF and EKF perform well under weak nonlinear conditions but degrade in strongly nonlinear maneuvers. UKF and CKF methods exhibit stronger robustness, with CKF5 achieving the highest accuracy, while CKF3 and UKF provide a better balance between accuracy and computational cost. This study provides practical guidelines for filter selection in USV path-following control.

Keywords

Unmanned surface vehicle / Path following control / State estimation / Extended kalman filter / Unscented kalman filter / Cubature Kalman filter

References

[1] Arasaratnam I, Haykin S. Cubature Kalman filters. IEEE Trans Autom Control, 2009, 54(6): 1254-1269

[2] Bar-Shalom Y, Li XR, Kirubarajan TEstimation with applications to tracking and navigation: Theory, algorithms and software, 2002New York, USJohn Wiley & Sons, Inc.1-545

[3] Benham D, Potokar E, Mangelson JG (2025) Invariant extended Kalman filter for autonomous surface vessels with partial orientation measurements. 2025 IEEE ICRA Workshop on Field Robotics, arXiv:2506.10850. http://arxiv.org/abs/2506.10850

[4] Berg HS, Menges D, Tengesdal T, Rasheed A. Digital twin syncing for autonomous surface vessels using reinforcement learning and nonlinear model predictive control. Sci. Rep., 2025, 15(1): 9344

[5] Bucy R. Nonlinear filtering theory. IEEE Trans Autom Control, 1965, 10(2): 198-198

[6] Chislett MS, Strom-Tejsen J. Planar motion mechanism tests and full-scale steering and manoeuvring predictions for a mariner class vessel. Int. Shipbuild. Prog., 1965, 12(129): 201-224

[7] Farrell JAided navigation: GPS with high rate sensors, 2008New York, USMcGraw-Hill, Inc.1-530

[8] Fossen S, Fossen TI. Five-state extended Kalman filter for estimation of speed over ground (SOG), course over ground (COG) and course rate of unmanned surface vehicles (USVs): Experimental results. Sens., 2021, 21(23): 7910

[9] Fossen TI. Line-of-sight path-following control utilizing an extended Kalman filter for estimation of speed and course over ground from GNSS positions. J. Mar. Sci. Technol., 2022, 27(1): 806-813

[10] Fossen TI, Breivik M, Skjetne R. Line-of-sight path following of underactuated marine craft. IFAC Proceedings Volumes, 2003, 36(21): 211-216

[11] Gade K. The seven ways to find heading. The Journal of Navigation, 2016, 69(5): 955-970

[12] He Z, Fan Y, Wang G, Mu D. Cooperative trajectory tracking control of MUSVs with periodic relative threshold event-triggered mechanism and safe distance. Ocean Eng., 2023, 269: 113541

[13] He Z, Fan Y, Wang G, Qiao S. Finite time course keeping control for unmanned surface vehicles with command filter and rudder saturation. Ocean Eng., 2023, 280: 114403

[14] He Z, Wang G, Fan Y, Qiao S. Fast finite-time path-following control of unmanned surface vehicles with sideslip compensation and time-varying disturbances. J. Mar. Sci. Eng., 2022, 10(7): 960

[15] He Z, Wang G, Fan Y, Qiao S. Fixed threshold event-triggered adaptive sliding mode trajectory tracking control of unmanned surface vehicles. 2022 IEEE 17th Conference on Industrial Electronics and Applications (ICIEA), Chengdu, China, 202233-39

[16] Huang T, Xue Y, Xue Z, Zhang Z, Miao Z, Liu Y. USV-tracker: A novel USV tracking system for surface investigation with limited resources. Ocean Eng., 2024, 312: 119196

[17] Jin XB, Jeremiah RJR, Su TL, Bai YT, Kong JL. The new trend of state estimation: From model-driven to hybrid-driven methods. Sens., 2021, 21(6): 2085

[18] Jin XB, Sun S, Wei H, Yang FB. Advances in multi-sensor information fusion: Theory and applications 2017. Sens., 2018, 18(4): 1162

[19] Julier SJ, Uhlmann JK. Unscented filtering and nonlinear estimation. Proc. IEEE, 2004, 92(3): 401-422

[20] Kalman RE. A new approach to linear filtering and prediction problems. J. Basic. Eng., 1960, 82(1): 35-45

[21] Kartika Aji AA, Sebastian M, Alexander F, Tsai PT, Lee MFR. State estimation for autonomous surface vehicles: A neural network-assisted observer approach. 2024 International Conference on Advanced Robotics and Intelligent Systems (ARIS), Taipei, China, 20241-6

[22] Li M, Guo C, Yu H. Global finite-time control for coordinated path following of multiple underactuated unmanned surface vehicles along one curve under directed topologies. Ocean Engineering, 2021, 237: 109608

[23] Li M, Guo C, Yu H, Yuan Y. Line-of-sight-based global finite-time stable path following control of unmanned surface vehicles with actuator saturation. ISA Transactions, 2022, 125: 306-317

[24] Li ZM, Yang WG, Ding D, Liao YR. High-degree spherical simplex-radial cubature quadrature Kalman filter. Tongxin Xuebao/Journal on Communications, 2017, 38: 111-117

[25] Liu W, Liu Y, Bucknall R. Filtering based multi-sensor data fusion algorithm for a reliable unmanned surface vehicle navigation. J. Mar. Eng. Technol., 2023, 22(2): 67-83

[26] McCue L. Handbook of marine craft hydrodynamics and motion control [Bookshelf]. IEEE Control Syst. Mag., 2016, 36(1): 78-79

[27] Nomoto K, Taguchi T, Honda K, Hirano S. On the steering qualities of ships. Int. Shipbuild. Prog., 1957, 4: 354-370

[28] Ouelmokhtar H, Benmoussa Y, Benazzouz D, Ait-Chikh MA, Lemarchand L. Energy-based USV maritime monitoring using multi-objective evolutionary algorithms. Ocean Eng., 2022, 253: 111182

[29] Pei Y, Biswas S, Fussell D, Pingali K. An elementary introduction to Kalman filtering. Commun. ACM, 2017, 62(11): 122-133

[30] Simon DOptimal state estimation: Kalman, H_∞, and nonlinear approaches, 2006New York, USWiley& Sons, Inc.1-552

[31] Specht M. Testing and analysis of selected navigation parameters of the GNSS/INS system for USV path localization during inland hydrographic surveys. Sens., 2024, 24(8): 18

[32] Wan EA, Van Der Merwe R. The unscented Kalman filter for nonlinear estimation. IEEE 2000 Adaptive Systems for Signal Processing, Communications, and Control Symposium (Cat. No. 00EX373), Lake Louise, Canada, 2000153-158

[33] Wang C, Liang X, Yu C, Liu D, Li W. Fixed-time event-triggered sliding mode cooperative path-following control with prescribed performance for USVs based on lumped disturbance observer. Ocean Eng., 2023, 289: 116123

[34] Wang J, Xia L, Peng L, Li H, Cui Y. Efficient uncertainty propagation in model-based reinforcement learning unmanned surface vehicle using unscented Kalman filter. Drones, 2023, 7(4): 28

[35] Wang N, Sun Z, Zheng Z, Zhao H. Finite-time sideslip observer-based adaptive fuzzy path-following control of underactuated marine vehicles with time-varying large sideslip. Int. J. Fuzzy Syst., 2018, 20(6): 1767-1778

[36] Wang S, Feng J, Tse CK. Spherical simplex-radial cubature Kalman filter. IEEE Signal Process. Lett., 2014, 21(1): 43-46

[37] Yan X, Yang X, Xiang Z, Shen H. Enhanced adaptability of UWB/INS for unmanned surface vehicles in GPS-denied environments: Ranging error identification and compensation. Ocean Eng., 2026, 344: 123695

[38] Yan Y, Yu S, Gao X, Wu D, Li T. Continuous and periodic event-triggered sliding-mode control for path following of underactuated surface vehicles. IEEE Trans. Cybern., 2024, 54(1): 449-461

RIGHTS & PERMISSIONS

Harbin Engineering University and Springer-Verlag GmbH Germany, part of Springer Nature