Joint state and environmental disturbance estimation via particle filtering for NMPC trajectory tracking of underwater vehicles

Ocean currents dominate the disturbance acting on underwater vehicles, yet existing compensation schemes rely on Kalman filtering, whose Gaussian noise assumption is violated by the impulsive measurement outliers characteristic of acoustic positioning. In this paper, an augmented-state particle filter is developed that jointly estimates the vehicle states and an equivalent environmental disturbance; the estimate is fed forward into the prediction model of a nonlinear model predictive controller and compensated proactively over the prediction horizon. Unlike schemes built on Kalman filtering, the proposed estimator does not rely on a Gaussian noise assumption and retains a finite estimate in the heavy-tailed regime where the extended-state Kalman filter diverges. Fifty-run Monte Carlo simulations are performed under constant, slowly-varying and abrupt disturbances. Relative to an extended-state Kalman filter baseline, the disturbance-estimation RMSE is reduced by 46.9%, 30.6% and 6.7%, and the tracking RMSE by 25.6%, 21.9% and 9.3% (Wilcoxon signed-rank test, 𝑝 < 0 . 0 5 throughout); relative to an uncompensated controller the tracking RMSE is reduced by 59.8%, 47.0% and 35.3%. A vectorised implementation requires 3.06 ms per filter update at 10 000 particles, so that the complete control cycle occupies about 21% of the 100 ms sampling period. All results reported in this paper are obtained by numerical simulation.