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

Authors: Yuexin Zhang; Lihui Wang
Journal: Mathematical Problems in Engineering
 (2018)

---

## Abstract

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.


---

## Body

## 1. 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–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 high-precision 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 model, and then the estimated disturbances are used to compensate the dynamic model. The disturbances estimation algorithm consists of two parts, nonlinear filter and disturbances nonlinear estimator. The nonlinear filter is used to provide necessary parameters for the following disturbances nonlinear estimator. For the dynamic model, extended Kalman filter [14], unscented Kalman filter [15], cubature Kalman filter (CKF) [16], embedded cubature Kalman filter [17], interpolatory cubature Kalman filter [18], stochastic integral filter [19], and other types of nonlinear filters can achieve similar effect. The proposed nonlinear estimator estimates the external disturbances as an unknown variable without establishing additional disturbances model or extend state. In view of the focus of the paper which is disturbances nonlinear estimator rather than the nonlinear filter, the paper uses a common CKF, and the detailed derivation process of nonlinear estimator is given in the appendix.The proposed method consists of disturbances estimation and disturbances compensation, which is named DEAC. The disturbances estimation is based on CKF and disturbances nonlinear estimator. The CKF is used to suppress noise, then the residual innovation sequences, gain matrix, and innovation covariance generated by CKF are employed estimate disturbances by using a nonlinear estimator. The disturbances compensation is carried out by brought the estimated disturbances into the system state equation, so the dynamic model is improved, and the estimated state values are more accurate. To verify the effectiveness of the DEAC, simulation experiments are carried out in three cases, namely, sinusoidal disturbances, random disturbances, and model parameters uncertainty.


## 2. 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=uvwT and the angular velocity υ2=pqrT are defined in the body frame, where u is the forward velocity, v is the right velocity, w is the down velocity, p is the pitch angular velocity, q is the roll angular velocity, and r is the yaw angular velocity. The position η1=xyzT and orientation η2=ϕθψT are defined in the navigation frame, where x is the north position, y is the east position, z 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)Mυ˙+Cυυ+Dυυ=τc+τdwhere υ=υ1Tυ2TT consists of the linear velocity υ1 and the angular velocity υ2; M∈R6×6 is the inertia matrix including the inertia matrix of rigid body MRB and the virtual inertia of hydrodynamic MA; Cυ∈R6×6 is the Coriolis centripetal matrix as a sum of rigid body terms CRBυ and terms CAυ due to drag forces, Dυ∈R6×6 is the damping matrix including terms due to drag forces, and Cυ and Dυ are nonlinear terms; τc is a vector of the forces and moments exerted on the vehicle by the controller and τd=τdu,τdv,τdw,τdp,τdq,τdrT is the unknown external disturbances, where τdu, τdv, and τdw are the disturbance forces in three directions and τdp, τdq, and τdr are the disturbance moments in three directions.The rigid body mass matrixMRB and the Coriolis centripetal matrix CRBυ are given as(2)MRB=m000mzG-myG0m0-mzG0mxG00mmyG-mxG00-mzGmyGIx00mzG0-mxG0IY0-myGmxG000IZ(3)CRBυ=000myGq+zGr-mxGq-w-mxGr+v000-myGp+wmzGr+xGp-myGr-u000-mzGp-v-mzGq+umxGp+yGq-myGq+zGrmyGp+wmzGp-v0IZr-IYqmxGq-w-mzGr+xGpmzGq+u-IZr0IXpmxGr+vmyGr-u-mxGp+yGqIYq-IXp0where m is the vehicle’s mass, xG,yG,zG is the center of gravity in Cartesian coordinate of body frame and IX, IY, and IZ are the moments of inertia and product of inertia terms for the related axes.The hydrodynamic added mass matrixMA and the corresponding hydrodynamic Coriolis centripetal matrix CAυ are given as(4)MA=-Xu˙000000Yv˙0Yp˙0Yr˙00Zw˙0Zq˙00Kv˙0Kp˙0Kr˙00Mw˙0Mq˙00Nv˙0Np˙0Nr˙(5)CAυ=0000-Zw˙wYv˙v000Zw˙w0-Xu˙u000-Yv˙vXu˙u00-Zw˙wYv˙v0-Nr˙rMq˙qZw˙w0-Xu˙uNr˙r0-Kp˙p-Yv˙vXu˙u0-Mq˙qKp˙p0where Xu˙, Yv˙, Yp˙, Yr˙, Zw˙, Zq˙, Kv˙, Kp˙, Kr˙, Mw˙, Mq˙, Nv˙, Np˙, and Nr˙ are acceleration hydrodynamic coefficients, usually obtained by computational fluid dynamic methods or towing tank experimental data analysis.The damping coefficient matrixDυ can be described as(6)Dυ=-diagXu+Xuuu,Yv+Yvvv,Zw+Zwww,Kp+Kppp,Mq+Mqqq,Nr+Nrrrwhere Xu, Yv, Zw, Kp, Mw, and Nr are velocity hydrodynamic coefficients; Xuu, Yvv, Zww, Kpp, Mqq, and Nrr are nonlinear coherent coefficients, usually obtained by empirical formulas.The vehicle kinematic equation is given as(7)η˙=Jηυwhere η=η1Tη2TT is the displacement vector under the navigation frame and Jη is an Euler angle transformation matrix defined as(8)Jη=J103×303×3J2where J1=cψcθ-sψcϕ+cψsθsϕsψsϕ+cψcϕsθsψcθcψcϕ+sϕsθsψ-cψsϕ+sθsψcϕ-sθcθsϕcθcϕ, J2=1sθtϕcϕtθ0cϕ-sϕ0sϕ/cθcϕ/cθ, and s∙=sin∙, c∙=cos∙, t∙=tan∙.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.(9)X˙=fX+Bτc+τdZ=HXwhere X=ηTυTT represents the system state vector, f∙ is a nonlinear function with respect to X, B is the system input matrix, Z is measurement vector of the system, and H is the system measurement matrix.(10)fX=06×6Jη06×6φυ,B=06×6M-1,H=03×6I3×303×3.where φυ=-M-1Cυ+Dυ, 06×6 is a 6×6 zero matrix, and I6×6 is a 6×6 unit matrix.Discretize (9); the discrete model can be described by(11)Xk=gXk-1,Fk-1+Wk-1Zk=hXk+Vkwhere Xk and Zk are the state vector and measurement vector, respectively, Fk-1 is the input vector including known force τc,k-1 and unknown disturbance τd,k-1, g∙ and h∙ are nonlinear functions, Wk-1 is the system white noise with mean being zero and covariance matrix being Qk-1, and Vk is the measurement white noise with mean being zero and covariance matrix being Rk. 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.


## 3. 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 Figure1.Figure 1
Block diagram of the proposed method.


### 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 stateX0/0(12)X^0/0=EX0/0P0/0=EX0/0-X^0/0X0/0-X^0/0Twhere X^0/0 is the initial state vector and P0/0 is the initial covariance matrix.(2) Calculate the propagated cubature points(13)Xi,k/k-1∗=gXi,k-1/k-1,τk-1,k-1,i=1,2,…,2nwhere Xi,k/k-1∗ is the propagated cubature point, Xi,k-1/k-1 is the prior estimated state, and n is the dimension of state vector Xk.(3) Estimate the predicted state and the corresponding error covariance(14)X^k/k-1=12n∑i=12nXi,k/k-1∗Pk/k-1=12n∑i=12nXi,k/k-1∗Xi,k/k-1∗T-X^k/k-1X^k/k-1T+Qk-1where X^k/k-1 is the predicted state and Pk/k-1 is the covariance matrix.(4) Calculate the propagated cubature pointsYi,k/k-1(15)Yi,k/k-1=hXi,k/k-1where Xi,k/k-1=Sk/k-1ξi+X^k/k-1 is the cubature point and Sk/k-1 is the Cholesky decomposition of Pk/k-1, ξi=n×1i.(5) Estimate the predicted measurementZ^k/k-1, the corresponding innovation covariance matrix Pzz,k/k-1, and the cross covariance matrix Pxz,k/k-1(16)Z^k/k-1=12n∑i=12nYi,k/k-1Pzz,k/k-1=12n∑i=12nYi,k/k-1Yi,k/k-1T-Z^k/k-1Z^k/k-1T+RkPxz,k/k-1=12n∑i=12nXi,k/k-1Yi,k/k-1T-X^k/k-1Z^k/k-1T(6) With the new measurement vectorZk, the filter gain Kk, innovation value Z-k, the updated state vector X^k/k, and the updated covariance matrix Pk/k are calculated as follows:(17)Kk=Pxz,k/k-1Pzz,k/k-1-1Z-k=Zk-Z^k/k-1X^k/k=X^k/k-1+KkZ-kPk/k=Pk/k-1-KkPzz,k/k-1KkT


### 3.2. 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 valueX^k/k-1 is used to simplify the nonlinear system to a linear system, and the innovation covariance Pzz,k/k-1, innovation value Z-k, and filter gain Kk 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(18)Φk=∂gX^k/k-1∂X(19)Γk=∂gX^k/k-1∂τd(20)Hk=∂hX^k/k-1∂Xwhere Φk, Γk, and Hk are the first-order Taylor series expansion at the estimated state value X^k/k-1.(2) Calculate the sensitivity matrices(21)Bsk=HkΦkMsk-1+IΓk(22)Msk=I-KkHkΦkMsk-1+I(3) Calculate the gain matrix, external disturbances, and error covariance matrix(23)Kbk=γ-1Pbk-1BsTkBskγ-1Pbk-1BsTk+Pzz,k/k-1-1(24)τ^d,k=τ^d,k-1+KbkZ-k-Bskτ^d,k-1(25)Pbk=I-KbkBskγ-1Pbk-1where Kbk is the correction gain for updating τ^d,k, γ is a fading factor, τ^d,k is the estimated external disturbances, and Pbk is the error covariance matrix. The fading factor γ can be set to a constant value which coordinate estimation accuracy with adaptive capability.


### 3.3. 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(26)Xk=gXk-1,τc,k-1+Wk-1The input force of the system state equation is only controlled byτc,k-1, the model is not accurate, and they may reduce the precision of navigation. By the above disturbances estimator, the accurate total force F^k can be obtained, including the control force τc,k and the disturbance force τ^d,k. Then the system state equation becomes(27)Xk=gXk-1,F^k-1+Wk-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 stateX0/0(12)X^0/0=EX0/0P0/0=EX0/0-X^0/0X0/0-X^0/0Twhere X^0/0 is the initial state vector and P0/0 is the initial covariance matrix.(2) Calculate the propagated cubature points(13)Xi,k/k-1∗=gXi,k-1/k-1,τk-1,k-1,i=1,2,…,2nwhere Xi,k/k-1∗ is the propagated cubature point, Xi,k-1/k-1 is the prior estimated state, and n is the dimension of state vector Xk.(3) Estimate the predicted state and the corresponding error covariance(14)X^k/k-1=12n∑i=12nXi,k/k-1∗Pk/k-1=12n∑i=12nXi,k/k-1∗Xi,k/k-1∗T-X^k/k-1X^k/k-1T+Qk-1where X^k/k-1 is the predicted state and Pk/k-1 is the covariance matrix.(4) Calculate the propagated cubature pointsYi,k/k-1(15)Yi,k/k-1=hXi,k/k-1where Xi,k/k-1=Sk/k-1ξi+X^k/k-1 is the cubature point and Sk/k-1 is the Cholesky decomposition of Pk/k-1, ξi=n×1i.(5) Estimate the predicted measurementZ^k/k-1, the corresponding innovation covariance matrix Pzz,k/k-1, and the cross covariance matrix Pxz,k/k-1(16)Z^k/k-1=12n∑i=12nYi,k/k-1Pzz,k/k-1=12n∑i=12nYi,k/k-1Yi,k/k-1T-Z^k/k-1Z^k/k-1T+RkPxz,k/k-1=12n∑i=12nXi,k/k-1Yi,k/k-1T-X^k/k-1Z^k/k-1T(6) With the new measurement vectorZk, the filter gain Kk, innovation value Z-k, the updated state vector X^k/k, and the updated covariance matrix Pk/k are calculated as follows:(17)Kk=Pxz,k/k-1Pzz,k/k-1-1Z-k=Zk-Z^k/k-1X^k/k=X^k/k-1+KkZ-kPk/k=Pk/k-1-KkPzz,k/k-1KkT


## 3.2. 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 valueX^k/k-1 is used to simplify the nonlinear system to a linear system, and the innovation covariance Pzz,k/k-1, innovation value Z-k, and filter gain Kk 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(18)Φk=∂gX^k/k-1∂X(19)Γk=∂gX^k/k-1∂τd(20)Hk=∂hX^k/k-1∂Xwhere Φk, Γk, and Hk are the first-order Taylor series expansion at the estimated state value X^k/k-1.(2) Calculate the sensitivity matrices(21)Bsk=HkΦkMsk-1+IΓk(22)Msk=I-KkHkΦkMsk-1+I(3) Calculate the gain matrix, external disturbances, and error covariance matrix(23)Kbk=γ-1Pbk-1BsTkBskγ-1Pbk-1BsTk+Pzz,k/k-1-1(24)τ^d,k=τ^d,k-1+KbkZ-k-Bskτ^d,k-1(25)Pbk=I-KbkBskγ-1Pbk-1where Kbk is the correction gain for updating τ^d,k, γ is a fading factor, τ^d,k is the estimated external disturbances, and Pbk is the error covariance matrix. The fading factor γ can be set to a constant value which coordinate estimation accuracy with adaptive capability.


## 3.3. 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(26)Xk=gXk-1,τc,k-1+Wk-1The input force of the system state equation is only controlled byτc,k-1, the model is not accurate, and they may reduce the precision of navigation. By the above disturbances estimator, the accurate total force F^k can be obtained, including the control force τc,k and the disturbance force τ^d,k. Then the system state equation becomes(27)Xk=gXk-1,F^k-1+Wk-1


## 4. 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τd=0.1×τ×sin⁡0.5t+0.15×τ×sin⁡1.5t+0.08×τ×sin⁡2.5t; (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:X^0/0=zeros(12,1), P0/0=105×eye12, Pb0=10×eye6, Ms0=10×eye12, τ^d,0=zeros6,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.Table 1
Principal dimensions of the underwater vehicle.
symbol value symbol value symbol value m 43.7 kg X u ˙ -27.08 kg X u u -61.117 kg/m2 x G , y G 0m Y v ˙ -25.952 kg Y v v -139.81 kg/m2 z G 0.026 m Z w ˙ -68.576 kg Z w w -51.724 kg/m2 I X 0.3319 kgm2 K p ˙ -61.683 kgm2/rad K p p -12 kgm2/rad2 I Y 1.4844 kgm2 M q ˙ -79.411 kgm2/rad M q q -56.61 kgm2/rad2 I Z 1.7596 kgm2 N r ˙ -0.154 kgm2/rad N r r -1.772 kgm2/rad2Root mean square error (RMSE) is used to evaluate the accuracy and described as follows(28)RMSE=1N∑i=1Nx^i-xi2where x^i is the estimated value and xi is the exact value.(1  ) Sinusoidal Disturbances. In the first numerical simulation, the input force vector acting on the vehicle is τc=20N,40N,0N,0N·m,0N·m,1N·mT and the disturbances vector is τd=0.1×τ×sin⁡0.5t+0.15×τ×sin⁡1.5t+0.08×τ×sin⁡2.5t.Figure2 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.Figure 2
Estimations of sinusoidal disturbances.
(a)
τ d u (b)
τ d v (c)
τ d rFigure 3
Position and orientation errors under sinusoidal disturbances.
(a)
x e (b)
y e (c)
ψ eFigure 4
Velocity errors under sinusoidal disturbances.
(a)
u e (b)
v e (c)
r eTable2 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.Table 2
Comparison of RMSEs under sinusoidal disturbances.
RMSE Position and orientation Velocity x m y m ψ ° u m / s v m / s r ° / s without compensation 0.0414 0.0271 0.8041 0.0208 0.0302 1.011 DEAC 0.0345 0.0102 0.4891 0.0139 0.0174 0.5960(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(29)τ˙d=-Tc-1τd+Ψnwhere τd∈R6 is a vector of random disturbances, n∈R6 is a vector of zero-mean Gaussian white noise, Tc∈R6×6 is a diagonal matrix of positive bias time constants, and Ψ∈R6×6 is a diagonal matrix scaling the amplitude of n.In the second numerical simulation experiment, the bias time constants are chosen asTc=diag1000,1000,1000,1000,1000,1000 and the scaling amplitudes are chosen as Ψ=diag8,16,0,0,0,0.4. The input force vector acting on the vehicle is the same as that in the first experiment.Figure5 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.Figure 5
Estimations of random disturbances.
(a)
τ d u (b)
τ d v (c)
τ d rFigure 6
Position and orientation errors under random disturbances.
(a)
x e (b)
y e (c)
ψ eFigure 7
Velocity errors under random disturbances.
(a)
u e (b)
v e (c)
r eTable3 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.Table 3
Comparison of RMSEs under random disturbances.
RMSE Position and orientation Velocity x m y m ψ ° u m / s v m / s r ° / s without compensation 0.0353 0.0192 0.9422 0.0137 0.0147 0.3963 DEAC 0.0238 0.0178 0.2552 0.0066 0.0079 0.2558(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.Figure8 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 τdu,τdv, and τdr 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 uncertainty, respectively. Figure 10 shows the comparison of velocity errors of without compensation and DEAC under model parameters uncertainty, respectively. Figures 9 and 10 demonstrate that even if the model has some errors, the DEAC algorithm can still effectively improve the navigation accuracy.Figure 8
Estimations of disturbances under model parameters uncertainty.
(a)
τ d u (b)
τ d v (c)
τ d rFigure 9
Position and orientation errors under model parameters uncertainty.
(a)
x e (b)
y e (c)
ψ eFigure 10
Velocity errors under model parameters uncertainty.
(a)
u e (b)
v e (c)
r eTable4 further lists the comparison of RMSEs of position and velocity under model parameters uncertainty. As shown in Table 4, after the DEAC, the RMSEs of position and orientation under model parameters uncertainty are decreased by 26.11%, 62.87%, and 55.82%, respectively; the RMSEs of velocity are decreased by 43.07%, 36.6%, and 47.33%, respectively.Table 4
Comparison of RMSEs under model parameters uncertainty.
RMSE Position and orientation Velocity x m y m ψ ° u m / s v m / s r ° / s without compensation 0.0553 0.0387 1.9386 0.0305 0.0340 1.1796 DEAC 0.0408 0.0144 0.8565 0.0174 0.0216 0.6213By comparing Tables2 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.


## 5. 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 estimate environmental disturbances and compensate the nonlinear dynamic model. Simulation experimental results verify that high navigation accuracy and good reliability are achieved even in the presence of random disturbances and model uncertainties. This research has great value in dynamic model aided integrated navigation, and future studies will focus on the engineering applications in underwater vehicles navigation and control system.



---

*Source: 3073072-2018-09-24.xml*