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 uncertainmodel parameters. The results demonstrate that the proposed method can estimate disturbances effectively and improves navigation accuracy significantly.


Introduction
Due to high sensitivity, excellent convenience, and low cost, underwater vehicles have been used extensively in both military and civilian institutions [1,2].Advanced navigation and control systems for underwater vehicles are required and dynamic model plays an important role in navigation control system design.However, the system for underwater vehicles is subjected to stochastic environmental disturbances, such as winds, waves, and water currents.The disturbances produce external oscillations and large accelerations, reducing the performance of navigation and control systems [3,4].To improve the underwater vehicles' reliability and accuracy, real-time disturbances estimation and compensation are essential.
To suppress the technical challenges of model uncertainties and unknown external disturbances, sliding mode controllers (SMC) are used to improve trajectory tracking performance of underwater robots [5][6][7][8].Another approach dealing with the unknown disturbances is to design the observer to estimate the unknown disturbances, followed by the control design to compensate for the estimated disturbances.Such disturbances observers include sliding mode observer [9], high-gain observer [10], and extended state observer [11].These methods are applied to highprecision controller in trajectory tracking or station keeping for autopilot application [12], but the effect of unknown external disturbances on the navigation system is not taken into account, especially dynamic model aided integrated navigation system.The dynamic model describes the vehicle motion with considering the forces and moments causing it.If the dynamic model does not consider external environmental forces and moments, the precision of dynamic model aided navigation will be reduced [13].To improve navigation performance, disturbances estimation algorithm is proposed to estimate the external disturbances of dynamic

Dynamic Model of Underwater Vehicles
The dynamic model describes the vehicle motion with considering the forces and moments causing it.The underwater vehicle sailing under the sea has six degrees of freedom such as surging, swaying, heaving, rolling, pitching, and yawing.Two reference frames are used to describe the model, the body frame (forward-right-down frame) and the navigation frame (north-east-down frame).The linear velocity  1 = [ V ] and the angular velocity  2 = [  ]  are defined in the body frame, where  is the forward velocity, V is the right velocity,  is the down velocity,  is the pitch angular velocity,  is the roll angular velocity, and  is the yaw angular velocity.The position  1 = [  ]  and orientation are defined in the navigation frame, where  is the north position,  is the east position,  is the down position,  is the pitch angle,  is the roll angle, and  is the yaw angle.
The dynamic model is a nonlinear model containing the Coriolis centripetal matrix and nonlinear damping term, which has the characteristics of external disturbances and uncertainty of model parameters.The nonlinear dynamics of the underwater vehicle with respect to the body frame is given as [20]  υ +  ()  +  ()  =   +   (1) where consists of the linear velocity  1 and the angular velocity  2 ;  ∈ R 6×6 is the inertia matrix including the inertia matrix of rigid body   and the virtual inertia of hydrodynamic   ; () ∈ R 6×6 is the Coriolis centripetal matrix as a sum of rigid body terms   () and terms   () due to drag forces, () ∈ R 6×6 is the damping matrix including terms due to drag forces, and () and () are nonlinear terms;   is a vector of the forces and moments exerted on the vehicle by the controller and   = [  ,  V ,   ,   ,   ,   ]  is the unknown external disturbances, where   ,  V , and   are the disturbance forces in three directions and   ,   , and   are the disturbance moments in three directions.
The rigid body mass matrix   and the Coriolis centripetal matrix   () are given as where  is the vehicle's mass, (  ,   ,   ) is the center of gravity in Cartesian coordinate of body frame and   ,   , and   are the moments of inertia and product of inertia terms for the related axes.The hydrodynamic added mass matrix   and the corresponding hydrodynamic Coriolis centripetal matrix   () are given as where where is the displacement vector under the navigation frame and () is an Euler angle transformation matrix defined as where ], and The state equation and the measurement equation can be obtained by transforming the nonlinear mathematical model of ( 1) and (7) into the state space model.
where  = [    ]  represents the system state vector, (•) is a nonlinear function with respect to ,  is the system input matrix,  is measurement vector of the system, and  is the system measurement matrix.
where () = − −1 (() + ()), 0 6×6 is a 6 × 6 zero matrix, and I 6×6 is a 6 × 6 unit matrix.Discretize ( 9); the discrete model can be described by where   and   are the state vector and measurement vector, respectively,  −1 is the input vector including known force  , −1 and unknown disturbance  ,−1 , (•) and ℎ(•) are nonlinear functions,  −1 is the system white noise with mean being zero and covariance matrix being  −1 , and   is the measurement white noise with mean being zero and covariance matrix being   .It is worth mentioning that, in practical engineering applications, Gaussian white noise is the most common form of noise, and most papers use Gaussian white noise as the processing object.For colored noise, many scholars have conducted special research on robust nonlinear filters.Since the focus of the paper is the disturbances nonlinear estimator described rather than the nonlinear filter, the paper does not consider colored noise.

Disturbances Estimation and Compensation
Underwater vehicles are subjected to known controller's forces and unknown external disturbances, causing the change of the motion state.The navigation system state space model is established based on nonlinear dynamic model.Sensors provide measured velocity for DEAC.The DEAC estimates the disturbances and compensates the dynamic model, providing a more accurate estimated state for navigation system.The nonlinear estimator estimates the disturbances by using the innovation covariance, innovation value, and filter gain obtained by the CKF equations.Then the dynamic model is improved with the estimated disturbances, and the estimated state values are compensated.The block diagram of the proposed method is shown in Figure 1.3.1.CKF.The CKF has been proposed for nonlinear state estimation and is a Gaussian approximation of a Bayesian filter [21].The CKF calculates the innovation covariance, innovation value and filter gain for the following nonlinear estimator.The calculation process of CKF is as follows [22].
(1) Initialize the filter by setting the initial state  0/0 where X0/0 is the initial state vector and  0/0 is the initial covariance matrix.
(2) Calculate the propagated cubature points where  * ,/−1 is the propagated cubature point,  ,−1/−1 is the prior estimated state, and  is the dimension of state vector   .
(3) Estimate the predicted state and the corresponding error covariance where X/−1 is the predicted state and  /−1 is the covariance matrix.

Disturbances Nonlinear Estimator.
In view of the principle of Kalman filter, the nonlinear estimator is proposed based on least square algorithm.It only needs the recent measurement values and the previously estimated values to be kept in storage.This characteristic can save considerable memory and greatly decrease the system burden.The nonlinear estimator estimates the external disturbances as an unknown variable without establishing additional disturbances model or extend state.In the process, the first-order Taylor series expansion around the estimated stated value X/−1 is used to simplify the nonlinear system to a linear system, and the innovation covariance  ,/−1 , innovation value   , and filter gain   generated by the CKF are used to estimate the disturbances.The detailed derivation of the disturbances estimation equations can be found in Appendix, and the simple calculation procedure is as follows: (1) Calculate Jacobean matrices where Φ  , Γ  , and   are the first-order Taylor series expansion at the estimated state value X/−1 .
(2) Calculate the sensitivity matrices where   () is the correction gain for updating τ, ,  is a fading factor, τ, is the estimated external disturbances, and   () is the error covariance matrix.The fading factor  can be set to a constant value which coordinate estimation accuracy with adaptive capability.

Disturbances Compensation.
Based on the above nonlinear disturbances estimator, the state model is improved with the estimated disturbances.The navigation state of underwater vehicles can be estimated precisely.
If the disturbances are unknown, the system state equation is given as The input force of the system state equation is only controlled by  ,−1 , the model is not accurate, and they may reduce the precision of navigation.By the above disturbances estimator, the accurate total force F can be obtained, including the control force  , and the disturbance force τ, .Then the system state equation becomes

Numerical Simulations and Analysis
In practical engineering applications, external disturbances are diverse.In order to fully verify the effectiveness of the DEAC, numerical simulations under three different conditions are carried out: (1) sinusoidal disturbances.Considering that most continuous signal can be represented as a combination of sinusoidal signals of different frequencies, in the first numerical simulation, the disturbances vector is set to   = 0.1×× sin(0.5)+0.15× × sin(1.5)+0.08×× sin(2.5);(2) random disturbances.Random disturbances can be used to simulate the slowly varying disturbances in real applications, so the 1st-order Markow model that has been considered in Fossen (1999) [23] is utilized in the second numerical simulation; (3) model parameters uncertainty and external disturbances.The parameters of the underwater vehicles model are usually not uncertain, especially the hydrodynamic parameters.To better reflect the real situation, referring to [24], hydrodynamic parameters uncertainty are set to 5% in the third simulation experiment.The initial conditions are generally listed as follows: X0/0 = (12, 1),  0/0 = 10 5 ×(12),   (0) = 10×(6),   (0) = 10 ×  (12), τ,0 = (6, 1), and  = 0.6.The principal dimensions of the underwater vehicle are listed in Table 1, and other hydrodynamic parameters are zero.Sensors are used to measure the velocity of the vehicle and the measurement accuracy is 0.01m/s.
Root mean square error (RMSE) is used to evaluate the accuracy and described as follows where x is the estimated value and   is the exact value.
(1) Sinusoidal Disturbances.In the first numerical simulation, the input force vector acting on the vehicle is   = [20N, 40N, 0N, 0N ⋅ m, 0N ⋅ m, 1N ⋅ m]  and the disturbances vector is   = 0.1 ×  × sin(0.5)+ 0.15 ×  × sin(1.5)+ 0.08 ×  × sin(2.5).Figure 2 plots the exact value and estimated value of sinusoidal disturbances and amplifies partial.It can be seen that the DEAC can effectively estimate sinusoidal disturbances.Figure 3 presents the comparison of position and orientation errors of without compensation and DEAC under sinusoidal disturbances, respectively.Figure 4 shows the comparison of velocity errors of without compensation and DEAC under sinusoidal disturbances, respectively.Figures 3 and 4 demonstrate that the DEAC greatly improve the accuracy of navigation.
Table 2 further lists the RMSEs of positions and velocities under sinusoidal disturbances.As shown in Table 2, after the DEAC, the RMSEs of positions and orientation under sinusoidal disturbances are decreased by 16.54%, 62.51%, and 39.18%, respectively; the RMSEs of velocities are decreased by 32.94%, 42.22%, and 41.05%, respectively.
(2) Random Disturbances.A frequently used random disturbance for underwater vehicles control applications is the 1st-order Markow process.This model can be used to  describe environmental forces and moments due to wave drift, ocean currents, wind,and unmodeled dynamics.The 1st-order Markow process can in general be expressed as where   ∈ R 6 is a vector of random disturbances,  ∈ R 6 is a vector of zero-mean Gaussian white noise, T  ∈ R 6×6 is a diagonal matrix of positive bias time constants, and Ψ ∈ R 6×6 is a diagonal matrix scaling the amplitude of .
Figure 5 plots the exact value and estimated value of random disturbances.It indicates the estimated values are in excellent agreement with the exact values under random disturbances.Figure 6 shows the comparison of position and orientation errors of without compensation and DEAC under random disturbances, respectively.Figure 7 compares velocity errors of without compensation and DEAC under random disturbances.The results show that DEAC can greatly improve the estimated position and velocity accuracy.Table 3 summarizes the comparison of RMSEs of positions and velocities under random disturbances.As shown in Table 3, after the DEAC, the accuracy of positions, and orientation under random disturbances are decreased by 32.43%, 7.18%, and 72.91%; the RMSEs of velocities are decreased by 52.06%, 46.11% and 35.44%, respectively.
(3) The Model Parameters Uncertainty and External Disturbances.The parameters of the underwater vehicles model are usually not uncertain, especially the hydrodynamic parameters.The hydrodynamic parameters of the dynamic model are generally calculated by computational fluid dynamic methods or towing tank experimental data analysis.The data will have some errors, but the perturbation of the error model parameters is within a certain range.In the third simulation experiment the hydrodynamic parameters uncertainty are 5%, and other settings are the same as the first experiment.
Figure 8 plots the exact values and estimated values of sinusoidal disturbances under model parameters uncertainty.It can be seen that the DEAC has some errors, and the maximum errors of estimated disturbances   , V , and   are 1.3912N, 1.4529N, and 0.0397Nm, respectively.Figure 9 presents the comparison of position and orientation errors of without compensation and DEAC under model parameters   2 and 4, it can be found that the perturbation of the model parameters will affect the accuracy, but after DEAC, the effect is reduced.The simulation results demonstrate that the DEAC can effectively estimate disturbances in different conditions and greatly improve the navigation accuracy.

Conclusions
In order to improve navigation performance of dynamic model aided navigation system, the novel algorithm DEAC is presented.The proposed method is established on nonlinear Kalman filter and nonlinear estimator, which can effectively  where  * ,/−1 is the propagated cubature point,  ,−1/−1 is the prior estimated state, and  is the dimension of state vector   .
Estimate the predicted state and the corresponding error covariance: where X/−1 is the predicted state and  /−1 is the covariance matrix.With the new measurement vector   , the filter gain   , innovation value   , the updated state vector X/ , and the updated covariance matrix  / are calculated as follows: (A.24)

Figure 1 :
Figure 1: Block diagram of the proposed method.

Figure 3 :
Figure 3: Position and orientation errors under sinusoidal disturbances.

Figure 6 :
Figure 6: Position and orientation errors under random disturbances.

Table 1 :
Principal dimensions of the underwater vehicle.

Table 3 :
Comparison of RMSEs under random disturbances.

Table 4 :
Comparison of RMSEs under model parameters uncertainty.