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.
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 and the angular velocity are defined in the body frame, where is the forward velocity, 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 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]where consists of the linear velocity and the angular velocity ; is the inertia matrix including the inertia matrix of rigid body and the virtual inertia of hydrodynamic ; is the Coriolis centripetal matrix as a sum of rigid body terms and terms due to drag forces, 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 is the unknown external disturbances, where , , 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 aswhere 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 aswhere , , , , , , , , , , , , , and are acceleration hydrodynamic coefficients, usually obtained by computational fluid dynamic methods or towing tank experimental data analysis.
The damping coefficient matrix can be described aswhere , , , , , and are velocity hydrodynamic coefficients; , , , , , and are nonlinear coherent coefficients, usually obtained by empirical formulas.
The vehicle kinematic equation is given aswhere is the displacement vector under the navigation frame and is an Euler angle transformation matrix defined aswhere , , 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 , is a zero matrix, and is a unit matrix.
Discretize (9); the discrete model can be described bywhere and are the state vector and measurement vector, respectively, is the input vector including known force and unknown disturbance , and are nonlinear functions, is the system white noise with mean being zero and covariance matrix being , 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.
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 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 where is the initial state vector and is the initial covariance matrix.
(2) Calculate the propagated cubature pointswhere is the propagated cubature point, is the prior estimated state, and is the dimension of state vector .
(3) Estimate the predicted state and the corresponding error covariancewhere is the predicted state and is the covariance matrix.
(4) Calculate the propagated cubature points where is the cubature point and is the Cholesky decomposition of , .
(5) Estimate the predicted measurement , the corresponding innovation covariance matrix , and the cross covariance matrix
(6) With the new measurement vector , the filter gain , innovation value , the updated state vector , and the updated covariance matrix are calculated as follows:
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 value is used to simplify the nonlinear system to a linear system, and the innovation covariance , 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 matriceswhere , , and are the first-order Taylor series expansion at the estimated state value .
(2) Calculate the sensitivity matrices
(3) Calculate the gain matrix, external disturbances, and error covariance matrixwhere 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.
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
The input force of the system state equation is only controlled by , the model is not accurate, and they may reduce the precision of navigation. By the above disturbances estimator, the accurate total force can be obtained, including the control force and the disturbance force . Then the system state equation becomes
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 ; (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: , , , , , and . 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 followswhere 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 and the disturbances vector is .
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.
(a)
(b)
(c)
(a)
(b)
(c)
(a)
(b)
(c)
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 aswhere is a vector of random disturbances, is a vector of zero-mean Gaussian white noise, is a diagonal matrix of positive bias time constants, and is a diagonal matrix scaling the amplitude of .
In the second numerical simulation experiment, the bias time constants are chosen as and the scaling amplitudes are chosen as . The input force vector acting on the vehicle is the same as that in the first experiment.
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.
(a)
(b)
(c)
(a)
(b)
(c)
(a)
(b)
(c)
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 ,, 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 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.
(a)
(b)
(c)
(a)
(b)
(c)
(a)
(b)
(c)
Table 4 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.
By comparing Tables 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.
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.
Appendix
The disturbances estimation algorithm consists of two parts, nonlinear filter CKF and disturbances nonlinear estimator. The CKF is used to provide necessary parameters for the following disturbances nonlinear estimator. The nonlinear estimator estimates the external disturbances as an unknown variable without establishing additional disturbances model or extend state, which is the innovation of the proposed algorithm.
The nonlinear discrete system model is described as follows:where and are the state vector and measurement vector, respectively, is unknown disturbance, and are nonlinear functions, is the system white noise with mean being zero and covariance matrix being , and is the measurement white noise with mean being zero and covariance matrix being .
Initialize the filter by setting the initial state :where is the initial state vector and is the initial covariance matrix.
Calculate the propagated cubature pointswhere is the propagated cubature point, is the prior estimated state, and is the dimension of state vector .
Estimate the predicted state and the corresponding error covariance:where is the predicted state and is the covariance matrix.
Calculate the propagated cubature points :where is the cubature point, is the Cholesky decomposition of , and .
Estimate the predicted measurement , the corresponding innovation covariance matrix , and the cross covariance matrix :
With the new measurement vector , the filter gain , innovation value , the updated state vector , and the updated covariance matrix are calculated as follows:
A posteriori state estimate without unknown disturbance is
A posteriori state estimate with disturbance iswhere , , and .
Define the difference of the two a posteriori state estimate as follows:
Assume the disturbance begin with the time , then
At the time , (A.11) becomes
From (A.11), as , so (A.12) becomes
Define , then (A.13) becomes
From (A.11), for , can be expressed as
From (A.14) and (A.15), can be expressed as
Assume , then
From (A.15) and (A.17), can be expressed aswhere
The observed value of the residual sequence with disturbance can be described as
The observed value of the residual sequence without disturbance is described as
For different values of k,
(1) , , so .
(2) , , so .
In summary, where .
For , , where
Assume , is got from CKF. The variance of is given by
From (A.23), can be given as
The error covariance matrix is
Including forgetting factor , from (A.25), it can be got
From (A.22), (A.23), and (A.28), the following formulas can be obtained:where , , and .
From (A.26) and (A.27), the following formulas can be obtained:
Substituting (A.26) into (A.32), can be expressed aswhere .
Substituting (A.27) into (A.33), can be expressed as
Data Availability
The data used to support the findings of this study are available from the corresponding author upon request.
Conflicts of Interest
The authors declare no conflicts of interest.
Acknowledgments
The project was supported by following funds: National Key Research and Development Program (2016YFD0702000), National Natural Science Foundation of China (61773113 and 41704025), and Natural Science Foundation of Jiangsu Province (BK20160668).