Real-Time Disturbances Estimating and Compensating of Nonlinear Dynamic Model for Underwater Vehicles

To reduce the deviation caused by the stochastic environmental disturbances, estimating these disturbances is required to compensate the navigation system. Based on the idea of Kalman filter using least-squares algorithm for optimal estimation, a nonlinear disturbances estimator which can be perfectly integrated with cubature Kalman filter (CKF) is proposed. For the nonlinear disturbances estimator, the disturbances are estimated by gain matrix, innovation sequences, and innovation covariance generated by CKF. The disturbances estimating and compensating algorithm consists of three parts. Firstly, the navigation system state space model is established based on nonlinear dynamic model of six degrees of freedom. Secondly, the external disturbances are estimated by using CKF and a nonlinear estimator. Finally, the disturbances compensation is carried out by improving the system state equation. In view of the uncertainty of the dynamic model and the randomness of external disturbances, numerical simulation experiments are conducted in the circumstances of sinusoidal disturbances, random disturbances, and uncertain model parameters. The results demonstrate that the proposed method can estimate disturbances effectively and improves navigation accuracy significantly.